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
Hide whitespace changes
Inline Side-by-side
Showing with 2415 additions and 1740 deletions
Documentation/GCodes.md
View file @ b3cca544
...@@ -52,6 +52,11 @@ ...@@ -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. * 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) * M85 - Set inactivity shutdown timer with parameter S[seconds]. To disable set zero (default)
* M92 - Set axis_steps_per_unit - same syntax as G92 * 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 * M104 - Set extruder target temp
* M105 - Read current temp * M105 - Read current temp
* M106 - Fan on * M106 - Fan on
......
Documentation/changelog.md
View file @ b3cca544
### Version 4.2.7 ### Version 4.2.7
* Add M906 Set motor Currents for ALLIGATOR board * Add M906 Set motor Currents for ALLIGATOR board
* Add M408 JSON OUTPUT * Add M408 JSON OUTPUT
* Add Cartesian Correction Hysteresis and Zwooble
* Bug fix
### Version 4.2.6 ### Version 4.2.6
* Bug Fix * Bug Fix
......
MK/Configuration_Cartesian.h
View file @ b3cca544
...@@ -24,6 +24,7 @@ ...@@ -24,6 +24,7 @@
* - Axis accelleration * - Axis accelleration
* - Homing feedrate * - Homing feedrate
* - Hotend offset * - Hotend offset
* - Cartesian Correction
* *
* Basic-settings can be found in Configuration_Basic.h * Basic-settings can be found in Configuration_Basic.h
* Feature-settings can be found in Configuration_Feature.h * Feature-settings can be found in Configuration_Feature.h
...@@ -423,4 +424,37 @@ ...@@ -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 //#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 #endif
\ No newline at end of file
MK/MK.ino
View file @ b3cca544
...@@ -90,6 +90,10 @@ ...@@ -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. * 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) * M85 - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
* M92 - Set axis_steps_per_unit - same syntax as G92 * 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) * M100 - Watch Free Memory (For Debugging Only)
* M104 - Set extruder target temp * M104 - Set extruder target temp
* M105 - Read current temp * M105 - Read current temp
......
MK/base.h
View file @ b3cca544
...@@ -46,39 +46,23 @@ ...@@ -46,39 +46,23 @@
#include "module/motion/stepper.h" #include "module/motion/stepper.h"
#include "module/motion/stepper_indirection.h" #include "module/motion/stepper_indirection.h"
#include "module/motion/planner.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/temperature.h"
#include "module/temperature/thermistortables.h" #include "module/temperature/thermistortables.h"
#include "module/lcd/ultralcd.h" #include "module/lcd/ultralcd.h"
#include "module/lcd/buzzer.h"
#include "module/nextion/Nextion_lcd.h" #include "module/nextion/Nextion_lcd.h"
#include "module/sd/cardreader.h" #include "module/sd/cardreader.h"
#include "module/servo/servo.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #include "module/watchdog/watchdog.h"
#include "module/motion/vector_3.h" #include "module/blinkm/blinkm.h"
#if ENABLED(AUTO_BED_LEVELING_GRID)
#include "module/motion/qr_solve.h"
#endif
#endif
#if MB(ALLIGATOR) #if MB(ALLIGATOR)
#include "module/alligator/external_dac.h" #include "module/alligator/external_dac.h"
#endif #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) #if HAS(DIGIPOTSS)
#include <SPI.h> #include <SPI.h>
#endif #endif
...@@ -87,4 +71,8 @@ ...@@ -87,4 +71,8 @@
#include "module/fwtest/firmware_test.h" #include "module/fwtest/firmware_test.h"
#endif #endif
#if ENABLED(RFID_MODULE)
#include "module/mfrc522/MFRC522_serial.h"
#endif
#endif #endif
MK/module/HAL/fastio.h
View file @ b3cca544
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#ifndef _FASTIO_ARDUINO_H #ifndef _FASTIO_ARDUINO_H
#define _FASTIO_ARDUINO_H #define _FASTIO_ARDUINO_H
#include <avr/io.h>
/* /*
utility functions utility functions
......
MK/module/MK_Main.cpp
View file @ b3cca544
...@@ -4843,6 +4843,51 @@ inline void gcode_M92() { ...@@ -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]); 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 * M100 Free Memory Watcher
* *
...@@ -6470,7 +6515,7 @@ inline void gcode_M428() { ...@@ -6470,7 +6515,7 @@ inline void gcode_M428() {
sync_plan_position(); sync_plan_position();
#endif #endif
ECHO_LM(DB, "Offset applied."); ECHO_LM(DB, "Offset applied.");
LCD_ALERTMESSAGEPGM("Offset applied."); LCD_MESSAGEPGM("Offset applied.");
#if HAS(BUZZER) #if HAS(BUZZER)
enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200")); enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
#endif #endif
...@@ -7455,6 +7500,26 @@ void process_next_command() { ...@@ -7455,6 +7500,26 @@ void process_next_command() {
gcode_M85(); break; gcode_M85(); break;
case 92: // M92 Set the steps-per-unit for one or more axes case 92: // M92 Set the steps-per-unit for one or more axes
gcode_M92(); break; 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 case 104: // M104
gcode_M104(); break; gcode_M104(); break;
case 105: // M105 Read current temperature case 105: // M105 Read current temperature
...@@ -7469,12 +7534,6 @@ void process_next_command() { ...@@ -7469,12 +7534,6 @@ void process_next_command() {
case 109: // M109 Wait for temperature case 109: // M109 Wait for temperature
gcode_M109(); break; 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 110: break; // M110: Set line number - don't show "unknown command"
case 111: // M111 Set debug level case 111: // M111 Set debug level
gcode_M111(); break; gcode_M111(); break;
......
MK/module/blinkm/blinkm.cpp
View file @ b3cca544
/* /**
blinkm.cpp - Library for controlling a BlinkM over i2c * blinkm.cpp - Library for controlling a BlinkM over i2c
Created by Tim Koster, August 21 2013. * Created by Tim Koster, August 21 2013.
*/ */
#include "../../base.h" #include "../../base.h"
#if ENABLED(BLINKM) #if ENABLED(BLINKM)
#include "blinkm.h"
#include <Wire.h>
#include "blinkm.h" void SendColors(byte red, byte grn, byte blu) {
#include <Wire.h> Wire.begin();
Wire.beginTransmission(0x09);
void SendColors(byte red, byte grn, byte blu) { Wire.write('o'); //to disable ongoing script, only needs to be used once
Wire.begin(); Wire.write('n');
Wire.beginTransmission(0x09); Wire.write(red);
Wire.write('o'); //to disable ongoing script, only needs to be used once Wire.write(grn);
Wire.write('n'); Wire.write(blu);
Wire.write(red); Wire.endTransmission();
Wire.write(grn); }
Wire.write(blu);
Wire.endTransmission();
}
#endif // BLINKM #endif // BLINKM
MK/module/blinkm/blinkm.h
View file @ b3cca544
...@@ -4,8 +4,10 @@ ...@@ -4,8 +4,10 @@
*/ */
#ifndef BLINKM_H #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 #endif
MK/module/communication/communication.h
View file @ b3cca544
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
#define ER "Error: " // error for host #define ER "Error: " // error for host
#define WT "Wait" // wait for host #define WT "Wait" // wait for host
#define DB "Echo: " // message for user #define DB "Echo: " // message for user
#define DEB "Debug: " // message for debug
#define CFG "Config: " // config for host #define CFG "Config: " // config for host
#define INFO "Info: " // info for host #define INFO "Info: " // info for host
#define RESEND "Resend: " // resend 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) { ...@@ -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_CS() {WRITE(ST7920_CS_PIN, 1); u8g_10MicroDelay();}
#define ST7920_NCS() {WRITE(ST7920_CS_PIN,0);} #define ST7920_NCS() {WRITE(ST7920_CS_PIN, 0);}
#define ST7920_SET_CMD() {ST7920_SWSPI_SND_8BIT(0xf8);u8g_10MicroDelay();} #define ST7920_SET_CMD() {ST7920_SWSPI_SND_8BIT(0xf8);u8g_10MicroDelay();}
#define ST7920_SET_DAT() {ST7920_SWSPI_SND_8BIT(0xfa);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();} #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; ...@@ -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) 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 #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 // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head); 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; ...@@ -140,6 +140,7 @@ extern float max_z_jerk;
extern float max_e_jerk[EXTRUDERS]; extern float max_e_jerk[EXTRUDERS];
extern float mintravelfeedrate; extern float mintravelfeedrate;
extern unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS]; extern unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
extern long position[NUM_AXIS];
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
extern bool autotemp_enabled; extern bool autotemp_enabled;
......
MK/module/motion/qr_solve.cpp
View file @ b3cca544
#include "../../base.h" #include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID) #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" int i4_min(int i1, int i2)
#include <stdlib.h>
#include <math.h>
//# include "r8lib.h" /******************************************************************************/
/*
int i4_min(int i1, int i2) Purpose:
/******************************************************************************/
/*
Purpose:
I4_MIN returns the smaller of two I4's. I4_MIN returns the smaller of two I4's.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
29 August 2006 29 August 2006
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, int I1, I2, two integers to be compared. Input, int I1, I2, two integers to be compared.
Output, int I4_MIN, the smaller of I1 and I2. Output, int I4_MIN, the smaller of I1 and I2.
*/ */
{ {
return (i1 < i2) ? i1 : i2; return (i1 < i2) ? i1 : i2;
} }
double r8_epsilon(void) double r8_epsilon(void)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
R8_EPSILON returns the R8 round off unit. R8_EPSILON returns the R8 round off unit.
Discussion: Discussion:
R8_EPSILON is a number R which is a power of 2 with the property that, R8_EPSILON is a number R which is a power of 2 with the property that,
to the precision of the computer's arithmetic, to the precision of the computer's arithmetic,
1 < 1 + R 1 < 1 + R
but but
1 = ( 1 + R / 2 ) 1 = ( 1 + R / 2 )
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
01 September 2012 01 September 2012
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Output, double R8_EPSILON, the R8 round-off unit. Output, double R8_EPSILON, the R8 round-off unit.
*/ */
{ {
const double value = 2.220446049250313E-016; const double value = 2.220446049250313E-016;
return value; return value;
} }
double r8_max(double x, double y) double r8_max(double x, double y)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
R8_MAX returns the maximum of two R8's. R8_MAX returns the maximum of two R8's.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
07 May 2006 07 May 2006
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, double X, Y, the quantities to compare. Input, double X, Y, the quantities to compare.
Output, double R8_MAX, the maximum of X and Y. Output, double R8_MAX, the maximum of X and Y.
*/ */
{ {
return (y < x) ? x : y; return (y < x) ? x : y;
} }
double r8_abs(double x) double r8_abs(double x)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
R8_ABS returns the absolute value of an R8. R8_ABS returns the absolute value of an R8.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
07 May 2006 07 May 2006
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, double X, the quantity whose absolute value is desired. Input, double X, the quantity whose absolute value is desired.
Output, double R8_ABS, the absolute value of X. Output, double R8_ABS, the absolute value of X.
*/ */
{ {
return (x < 0.0) ? -x : x; return (x < 0.0) ? -x : x;
} }
double r8_sign(double x) double r8_sign(double x)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
R8_SIGN returns the sign of an R8. R8_SIGN returns the sign of an R8.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
08 May 2006 08 May 2006
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, double X, the number whose sign is desired. Input, double X, the number whose sign is desired.
Output, double R8_SIGN, the sign of X. Output, double R8_SIGN, the sign of X.
*/ */
{ {
return (x < 0.0) ? -1.0 : 1.0; 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: Purpose:
R8MAT_AMAX returns the maximum absolute value entry of an R8MAT. R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
Discussion: Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order. in column-major order.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
07 September 2012 07 September 2012
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, int M, the number of rows in A. Input, int M, the number of rows in A.
Input, int N, the number of columns in A. Input, int N, the number of columns in A.
Input, double A[M*N], the M by N matrix. Input, double A[M*N], the M by N matrix.
Output, double R8MAT_AMAX, the maximum absolute value entry of A. Output, double R8MAT_AMAX, the maximum absolute value entry of A.
*/ */
{ {
double value = r8_abs(a[0 + 0 * m]); double value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) { for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) { for (int i = 0; i < m; i++) {
NOLESS(value, r8_abs(a[i + j * m])); NOLESS(value, r8_abs(a[i + j * m]));
}
} }
return value;
} }
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: Purpose:
R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT. R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
Discussion: Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order. in column-major order.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
26 July 2008 26 July 2008
Author: Author:
John Burkardt John Burkardt
Parameters: Parameters:
Input, int M, N, the number of rows and columns. Input, int M, N, the number of rows and columns.
Input, double A1[M*N], the matrix to be copied. Input, double A1[M*N], the matrix to be copied.
Output, double R8MAT_COPY_NEW[M*N], the copy of A1. Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/ */
{ {
for (int j = 0; j < n; j++) { for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) for (int i = 0; i < m; i++)
a2[i + j * m] = a1[i + j * m]; 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. /******************************************************************************/
/*
Purpose:
Discussion: DAXPY computes constant times a vector plus a vector.
This routine uses unrolled loops for increments equal to one. Discussion:
Licensing: This routine uses unrolled loops for increments equal to one.
This code is distributed under the GNU LGPL license. Licensing:
Modified: This code is distributed under the GNU LGPL license.
30 March 2007 Modified:
Author: 30 March 2007
C version by John Burkardt Author:
Reference: C version by John Burkardt
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, Reference:
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
Basic Linear Algebra Subprograms for Fortran Usage, LINPACK User's Guide,
Algorithm 539, SIAM, 1979.
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters: Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Input, int N, the number of elements in DX and DY. Parameters:
Input, double DA, the multiplier of DX. Input, int N, the number of elements in DX and DY.
Input, double DX[*], the first vector. Input, double DA, the multiplier of DX.
Input, int INCX, the increment between successive entries of DX. Input, double DX[*], the first vector.
Input/output, double DY[*], the second vector. Input, int INCX, the increment between successive entries of DX.
On output, DY[*] has been replaced by DY[*] + DA * DX[*].
Input, int INCY, the increment between successive entries of DY. Input/output, double DY[*], the second vector.
*/ On output, DY[*] has been replaced by DY[*] + DA * DX[*].
{
if (n <= 0 || da == 0.0) return;
int i, ix, iy, m; Input, int INCY, the increment between successive entries of DY.
/*
Code for unequal increments or equal increments
not equal to 1.
*/ */
if (incx != 1 || incy != 1) { {
if (0 <= incx) if (n <= 0 || da == 0.0) return;
ix = 0;
else int i, ix, iy, m;
ix = (- n + 1) * incx; /*
if (0 <= incy) Code for unequal increments or equal increments
iy = 0; not equal to 1.
else */
iy = (- n + 1) * incy; if (incx != 1 || incy != 1) {
for (i = 0; i < n; i++) { if (0 <= incx)
dy[iy] = dy[iy] + da * dx[ix]; ix = 0;
ix = ix + incx; else
iy = iy + incy; ix = (- n + 1) * incx;
if (0 <= incy)
iy = 0;
else
iy = (- n + 1) * incy;
for (i = 0; i < n; i++) {
dy[iy] = dy[iy] + da * dx[ix];
ix = ix + incx;
iy = iy + incy;
}
} }
} /*
/* Code for both increments equal to 1.
Code for both increments equal to 1. */
*/ else {
else { m = n % 4;
m = n % 4; for (i = 0; i < m; i++)
for (i = 0; i < m; i++) dy[i] = dy[i] + da * dx[i];
dy[i] = dy[i] + da * dx[i]; for (i = m; i < n; i = i + 4) {
for (i = m; i < n; i = i + 4) { dy[i ] = dy[i ] + da * dx[i ];
dy[i ] = dy[i ] + da * dx[i ]; dy[i + 1] = dy[i + 1] + da * dx[i + 1];
dy[i + 1] = dy[i + 1] + da * dx[i + 1]; dy[i + 2] = dy[i + 2] + da * dx[i + 2];
dy[i + 2] = dy[i + 2] + da * dx[i + 2]; dy[i + 3] = dy[i + 3] + da * dx[i + 3];
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: Purpose:
DDOT forms the dot product of two vectors. DDOT forms the dot product of two vectors.
Discussion: Discussion:
This routine uses unrolled loops for increments equal to one. This routine uses unrolled loops for increments equal to one.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
30 March 2007 30 March 2007
Author: Author:
C version by John Burkardt C version by John Burkardt
Reference: Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide, LINPACK User's Guide,
SIAM, 1979. SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage, Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539, Algorithm 539,
ACM Transactions on Mathematical Software, ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323. Volume 5, Number 3, September 1979, pages 308-323.
Parameters: Parameters:
Input, int N, the number of entries in the vectors. Input, int N, the number of entries in the vectors.
Input, double DX[*], the first vector. Input, double DX[*], the first vector.
Input, int INCX, the increment between successive entries in DX. Input, int INCX, the increment between successive entries in DX.
Input, double DY[*], the second vector. Input, double DY[*], the second vector.
Input, int INCY, the increment between successive entries in DY. Input, int INCY, the increment between successive entries in DY.
Output, double DDOT, the sum of the product of the corresponding Output, double DDOT, the sum of the product of the corresponding
entries of DX and DY. entries of DX and DY.
*/ */
{ {
if (n <= 0) return 0.0; if (n <= 0) return 0.0;
int i, m; int i, m;
double dtemp = 0.0; double dtemp = 0.0;
/* /*
Code for unequal increments or equal increments Code for unequal increments or equal increments
not equal to 1. not equal to 1.
*/ */
if (incx != 1 || incy != 1) { if (incx != 1 || incy != 1) {
int ix = (incx >= 0) ? 0 : (-n + 1) * incx, int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
iy = (incy >= 0) ? 0 : (-n + 1) * incy; iy = (incy >= 0) ? 0 : (-n + 1) * incy;
for (i = 0; i < n; i++) { for (i = 0; i < n; i++) {
dtemp += dx[ix] * dy[iy]; dtemp += dx[ix] * dy[iy];
ix = ix + incx; ix = ix + incx;
iy = iy + incy; iy = iy + incy;
}
} }
} /*
/* Code for both increments equal to 1.
Code for both increments equal to 1. */
*/ else {
else { m = n % 5;
m = n % 5; for (i = 0; i < m; i++)
for (i = 0; i < m; i++) dtemp += dx[i] * dy[i];
dtemp += dx[i] * dy[i]; for (i = m; i < n; i = i + 5) {
for (i = m; i < n; i = i + 5) { dtemp += dx[i] * dy[i]
dtemp += dx[i] * dy[i] + dx[i + 1] * dy[i + 1]
+ dx[i + 1] * dy[i + 1] + dx[i + 2] * dy[i + 2]
+ dx[i + 2] * dy[i + 2] + dx[i + 3] * dy[i + 3]
+ dx[i + 3] * dy[i + 3] + dx[i + 4] * dy[i + 4];
+ dx[i + 4] * dy[i + 4]; }
} }
return dtemp;
} }
return dtemp; /******************************************************************************/
}
/******************************************************************************/
double dnrm2(int n, double x[], int incx) double dnrm2(int n, double x[], int incx)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
DNRM2 returns the euclidean norm of a vector. DNRM2 returns the euclidean norm of a vector.
Discussion: Discussion:
DNRM2 ( X ) = sqrt ( X' * X ) DNRM2 ( X ) = sqrt ( X' * X )
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
30 March 2007 30 March 2007
Author: Author:
C version by John Burkardt C version by John Burkardt
Reference: Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide, LINPACK User's Guide,
SIAM, 1979. SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage, Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539, Algorithm 539,
ACM Transactions on Mathematical Software, ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323. Volume 5, Number 3, September 1979, pages 308-323.
Parameters: Parameters:
Input, int N, the number of entries in the vector. Input, int N, the number of entries in the vector.
Input, double X[*], the vector whose norm is to be computed. Input, double X[*], the vector whose norm is to be computed.
Input, int INCX, the increment between successive entries of X. Input, int INCX, the increment between successive entries of X.
Output, double DNRM2, the Euclidean norm of X. Output, double DNRM2, the Euclidean norm of X.
*/ */
{ {
double norm; double norm;
if (n < 1 || incx < 1) if (n < 1 || incx < 1)
norm = 0.0; norm = 0.0;
else if (n == 1) else if (n == 1)
norm = r8_abs(x[0]); norm = r8_abs(x[0]);
else { else {
double scale = 0.0, ssq = 1.0; double scale = 0.0, ssq = 1.0;
int ix = 0; int ix = 0;
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
if (x[ix] != 0.0) { if (x[ix] != 0.0) {
double absxi = r8_abs(x[ix]); double absxi = r8_abs(x[ix]);
if (scale < absxi) { if (scale < absxi) {
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi); ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
scale = absxi; scale = absxi;
}
else
ssq = ssq + (absxi / scale) * (absxi / scale);
} }
else ix += incx;
ssq = ssq + (absxi / scale) * (absxi / scale);
} }
ix += incx; norm = scale * sqrt(ssq);
} }
norm = scale * sqrt(ssq); return norm;
} }
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[]) int jpvt[], double qraux[])
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
DQRANK computes the QR factorization of a rectangular matrix. DQRANK computes the QR factorization of a rectangular matrix.
Discussion: Discussion:
This routine is used in conjunction with DQRLSS to solve This routine is used in conjunction with DQRLSS to solve
overdetermined, underdetermined and singular linear systems overdetermined, underdetermined and singular linear systems
in a least squares sense. in a least squares sense.
DQRANK uses the LINPACK subroutine DQRDC to compute the QR DQRANK uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A. factorization, with column pivoting, of an M by N matrix A.
The numerical rank is determined using the tolerance TOL. The numerical rank is determined using the tolerance TOL.
Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns, of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL. and of R. This estimate will be <= 1/TOL.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
21 April 2012 21 April 2012
Author: Author:
C version by John Burkardt. C version by John Burkardt.
Reference: Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide, LINPACK User's Guide,
SIAM, 1979, SIAM, 1979,
ISBN13: 978-0-898711-72-1, ISBN13: 978-0-898711-72-1,
LC: QA214.L56. LC: QA214.L56.
Parameters: Parameters:
Input/output, double A[LDA*N]. On input, the matrix whose Input/output, double A[LDA*N]. On input, the matrix whose
decomposition is to be computed. On output, the information from DQRDC. decomposition is to be computed. On output, the information from DQRDC.
The triangular matrix R of the QR factorization is contained in the The triangular matrix R of the QR factorization is contained in the
upper triangle and information needed to recover the orthogonal upper triangle and information needed to recover the orthogonal
matrix Q is stored below the diagonal in A and in the vector QRAUX. matrix Q is stored below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A, which must Input, int LDA, the leading dimension of A, which must
be at least M. be at least M.
Input, int M, the number of rows of A. Input, int M, the number of rows of A.
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy, EPS. Then a reasonable of A have roughly the same absolute accuracy, EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest value for TOL is roughly EPS divided by the magnitude of the largest
element. element.
Output, int *KR, the numerical rank. Output, int *KR, the numerical rank.
Output, int JPVT[N], the pivot information from DQRDC. Output, int JPVT[N], the pivot information from DQRDC.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns independent to within the tolerance TOL and the remaining columns
are linearly dependent. are linearly dependent.
Output, double QRAUX[N], will contain extra information defining Output, double QRAUX[N], will contain extra information defining
the QR factorization. the QR factorization.
*/ */
{ {
double work[n]; double work[n];
for (int i = 0; i < n; i++) for (int i = 0; i < n; i++)
jpvt[i] = 0; jpvt[i] = 0;
int job = 1; int job = 1;
dqrdc(a, lda, m, n, qraux, jpvt, work, job); dqrdc(a, lda, m, n, qraux, jpvt, work, job);
*kr = 0; *kr = 0;
int k = i4_min(m, n); int k = i4_min(m, n);
for (int j = 0; j < k; j++) { for (int j = 0; j < k; j++) {
if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda])) if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
return; return;
*kr = j + 1; *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) double work[], int job)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
DQRDC computes the QR factorization of a real rectangular matrix.
Discussion: DQRDC computes the QR factorization of a real rectangular matrix.
DQRDC uses Householder transformations. Discussion:
Column pivoting based on the 2-norms of the reduced columns may be DQRDC uses Householder transformations.
performed at the user's option.
Licensing: Column pivoting based on the 2-norms of the reduced columns may be
performed at the user's option.
This code is distributed under the GNU LGPL license. Licensing:
Modified: This code is distributed under the GNU LGPL license.
07 June 2005 Modified:
Author: 07 June 2005
C version by John Burkardt. Author:
Reference: C version by John Burkardt.
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, Reference:
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Parameters: Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Input/output, double A(LDA,P). On input, the N by P matrix Parameters:
whose decomposition is to be computed. On output, A contains in
its upper triangle the upper triangular matrix R of the QR
factorization. Below its diagonal A contains information from
which the orthogonal part of the decomposition can be recovered.
Note that if pivoting has been requested, the decomposition is not that
of the original matrix A but that of A with its columns permuted
as described by JPVT.
Input, int LDA, the leading dimension of the array A. LDA must Input/output, double A(LDA,P). On input, the N by P matrix
be at least N. whose decomposition is to be computed. On output, A contains in
its upper triangle the upper triangular matrix R of the QR
factorization. Below its diagonal A contains information from
which the orthogonal part of the decomposition can be recovered.
Note that if pivoting has been requested, the decomposition is not that
of the original matrix A but that of A with its columns permuted
as described by JPVT.
Input, int N, the number of rows of the matrix A. Input, int LDA, the leading dimension of the array A. LDA must
be at least N.
Input, int P, the number of columns of the matrix A. Input, int N, the number of rows of the matrix A.
Output, double QRAUX[P], contains further information required Input, int P, the number of columns of the matrix A.
to recover the orthogonal part of the decomposition.
Input/output, integer JPVT[P]. On input, JPVT contains integers that Output, double QRAUX[P], contains further information required
control the selection of the pivot columns. The K-th column A(*,K) of A to recover the orthogonal part of the decomposition.
is placed in one of three classes according to the value of JPVT(K).
> 0, then A(K) is an initial column.
= 0, then A(K) is a free column.
< 0, then A(K) is a final column.
Before the decomposition is computed, initial columns are moved to
the beginning of the array A and final columns to the end. Both
initial and final columns are frozen in place during the computation
and only free columns are moved. At the K-th stage of the
reduction, if A(*,K) is occupied by a free column it is interchanged
with the free column of largest reduced norm. JPVT is not referenced
if JOB == 0. On output, JPVT(K) contains the index of the column of the
original matrix that has been interchanged into the K-th column, if
pivoting was requested.
Workspace, double WORK[P]. WORK is not referenced if JOB == 0. Input/output, integer JPVT[P]. On input, JPVT contains integers that
control the selection of the pivot columns. The K-th column A(*,K) of A
is placed in one of three classes according to the value of JPVT(K).
> 0, then A(K) is an initial column.
= 0, then A(K) is a free column.
< 0, then A(K) is a final column.
Before the decomposition is computed, initial columns are moved to
the beginning of the array A and final columns to the end. Both
initial and final columns are frozen in place during the computation
and only free columns are moved. At the K-th stage of the
reduction, if A(*,K) is occupied by a free column it is interchanged
with the free column of largest reduced norm. JPVT is not referenced
if JOB == 0. On output, JPVT(K) contains the index of the column of the
original matrix that has been interchanged into the K-th column, if
pivoting was requested.
Input, int JOB, initiates column pivoting. Workspace, double WORK[P]. WORK is not referenced if JOB == 0.
0, no pivoting is done.
nonzero, pivoting is done.
*/
{
int jp;
int j;
int lup;
int maxj;
double maxnrm, nrmxl, t, tt;
int pl = 1, pu = 0; Input, int JOB, initiates column pivoting.
/* 0, no pivoting is done.
If pivoting is requested, rearrange the columns. nonzero, pivoting is done.
*/ */
if (job != 0) { {
for (j = 1; j <= p; j++) { int jp;
int swapj = (0 < jpvt[j - 1]); int j;
jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j; int lup;
if (swapj) { int maxj;
if (j != pl) double maxnrm, nrmxl, t, tt;
dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
jpvt[j - 1] = jpvt[pl - 1]; int pl = 1, pu = 0;
jpvt[pl - 1] = j;
pl++;
}
}
pu = p;
for (j = p; 1 <= j; j--) {
if (jpvt[j - 1] < 0) {
jpvt[j - 1] = -jpvt[j - 1];
if (j != pu) {
dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
jp = jpvt[pu - 1];
jpvt[pu - 1] = jpvt[j - 1];
jpvt[j - 1] = jp;
}
pu = pu - 1;
}
}
}
/*
Compute the norms of the free columns.
*/
for (j = pl; j <= pu; j++)
qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
for (j = pl; j <= pu; j++)
work[j - 1] = qraux[j - 1];
/*
Perform the Householder reduction of A.
*/
lup = i4_min(n, p);
for (int l = 1; l <= lup; l++) {
/* /*
Bring the column of largest norm into the pivot position. If pivoting is requested, rearrange the columns.
*/ */
if (pl <= l && l < pu) { if (job != 0) {
maxnrm = 0.0; for (j = 1; j <= p; j++) {
maxj = l; int swapj = (0 < jpvt[j - 1]);
for (j = l; j <= pu; j++) { jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j;
if (maxnrm < qraux[j - 1]) { if (swapj) {
maxnrm = qraux[j - 1]; if (j != pl)
maxj = j; dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
jpvt[j - 1] = jpvt[pl - 1];
jpvt[pl - 1] = j;
pl++;
} }
} }
if (maxj != l) { pu = p;
dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1); for (j = p; 1 <= j; j--) {
qraux[maxj - 1] = qraux[l - 1]; if (jpvt[j - 1] < 0) {
work[maxj - 1] = work[l - 1]; jpvt[j - 1] = -jpvt[j - 1];
jp = jpvt[maxj - 1]; if (j != pu) {
jpvt[maxj - 1] = jpvt[l - 1]; dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
jpvt[l - 1] = jp; jp = jpvt[pu - 1];
jpvt[pu - 1] = jpvt[j - 1];
jpvt[j - 1] = jp;
}
pu = pu - 1;
}
} }
} }
/* /*
Compute the Householder transformation for column L. Compute the norms of the free columns.
*/ */
qraux[l - 1] = 0.0; for (j = pl; j <= pu; j++)
if (l != n) { qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1); for (j = pl; j <= pu; j++)
if (nrmxl != 0.0) { work[j - 1] = qraux[j - 1];
if (a[l - 1 + (l - 1)*lda] != 0.0) /*
nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]); Perform the Householder reduction of A.
dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1); */
a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda]; lup = i4_min(n, p);
/* for (int l = 1; l <= lup; l++) {
Apply the transformation to the remaining columns, updating the norms. /*
*/ Bring the column of largest norm into the pivot position.
for (j = l + 1; j <= p; j++) { */
t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1) if (pl <= l && l < pu) {
/ a[l - 1 + (l - 1) * lda]; maxnrm = 0.0;
daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1); maxj = l;
if (pl <= j && j <= pu) { for (j = l; j <= pu; j++) {
if (qraux[j - 1] != 0.0) { if (maxnrm < qraux[j - 1]) {
tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2); maxnrm = qraux[j - 1];
tt = r8_max(tt, 0.0); maxj = j;
t = tt; }
tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2); }
if (tt != 1.0) if (maxj != l) {
qraux[j - 1] = qraux[j - 1] * sqrt(t); dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1);
else { qraux[maxj - 1] = qraux[l - 1];
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1); work[maxj - 1] = work[l - 1];
work[j - 1] = qraux[j - 1]; jp = jpvt[maxj - 1];
jpvt[maxj - 1] = jpvt[l - 1];
jpvt[l - 1] = jp;
}
}
/*
Compute the Householder transformation for column L.
*/
qraux[l - 1] = 0.0;
if (l != n) {
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1);
if (nrmxl != 0.0) {
if (a[l - 1 + (l - 1)*lda] != 0.0)
nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]);
dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1);
a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda];
/*
Apply the transformation to the remaining columns, updating the norms.
*/
for (j = l + 1; j <= p; j++) {
t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1)
/ a[l - 1 + (l - 1) * lda];
daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1);
if (pl <= j && j <= pu) {
if (qraux[j - 1] != 0.0) {
tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2);
tt = r8_max(tt, 0.0);
t = tt;
tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2);
if (tt != 1.0)
qraux[j - 1] = qraux[j - 1] * sqrt(t);
else {
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
work[j - 1] = qraux[j - 1];
}
} }
} }
} }
/*
Save the transformation.
*/
qraux[l - 1] = a[l - 1 + (l - 1) * lda];
a[l - 1 + (l - 1)*lda] = -nrmxl;
} }
/*
Save the transformation.
*/
qraux[l - 1] = a[l - 1 + (l - 1) * lda];
a[l - 1 + (l - 1)*lda] = -nrmxl;
} }
} }
} }
} /******************************************************************************/
/******************************************************************************/
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) double x[], double rsd[], int jpvt[], double qraux[], int itask)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
DQRLS factors and solves a linear system in the least squares sense. DQRLS factors and solves a linear system in the least squares sense.
Discussion: Discussion:
The linear system may be overdetermined, underdetermined or singular. The linear system may be overdetermined, underdetermined or singular.
The solution is obtained using a QR factorization of the The solution is obtained using a QR factorization of the
coefficient matrix. coefficient matrix.
DQRLS can be efficiently used to solve several least squares DQRLS can be efficiently used to solve several least squares
problems with the same matrix A. The first system is solved problems with the same matrix A. The first system is solved
with ITASK = 1. The subsequent systems are solved with with ITASK = 1. The subsequent systems are solved with
ITASK = 2, to avoid the recomputation of the matrix factors. ITASK = 2, to avoid the recomputation of the matrix factors.
The parameters KR, JPVT, and QRAUX must not be modified The parameters KR, JPVT, and QRAUX must not be modified
between calls to DQRLS. between calls to DQRLS.
DQRLS is used to solve in a least squares sense DQRLS is used to solve in a least squares sense
overdetermined, underdetermined and singular linear systems. overdetermined, underdetermined and singular linear systems.
The system is A*X approximates B where A is M by N. The system is A*X approximates B where A is M by N.
B is a given M-vector, and X is the N-vector to be computed. B is a given M-vector, and X is the N-vector to be computed.
A solution X is found which minimimzes the sum of squares (2-norm) A solution X is found which minimimzes the sum of squares (2-norm)
of the residual, A*X - B. of the residual, A*X - B.
The numerical rank of A is determined using the tolerance TOL. The numerical rank of A is determined using the tolerance TOL.
DQRLS uses the LINPACK subroutine DQRDC to compute the QR DQRLS uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A. factorization, with column pivoting, of an M by N matrix A.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
10 September 2012 10 September 2012
Author: Author:
C version by John Burkardt. C version by John Burkardt.
Reference: Reference:
David Kahaner, Cleve Moler, Steven Nash, David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software, Numerical Methods and Software,
Prentice Hall, 1989, Prentice Hall, 1989,
ISBN: 0-13-627258-4, ISBN: 0-13-627258-4,
LC: TA345.K34. LC: TA345.K34.
Parameters: Parameters:
Input/output, double A[LDA*N], an M by N matrix. Input/output, double A[LDA*N], an M by N matrix.
On input, the matrix whose decomposition is to be computed. On input, the matrix whose decomposition is to be computed.
In a least squares data fitting problem, A(I,J) is the In a least squares data fitting problem, A(I,J) is the
value of the J-th basis (model) function at the I-th data point. value of the J-th basis (model) function at the I-th data point.
On output, A contains the output from DQRDC. The triangular matrix R On output, A contains the output from DQRDC. The triangular matrix R
of the QR factorization is contained in the upper triangle and of the QR factorization is contained in the upper triangle and
information needed to recover the orthogonal matrix Q is stored information needed to recover the orthogonal matrix Q is stored
below the diagonal in A and in the vector QRAUX. below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A. Input, int LDA, the leading dimension of A.
Input, int M, the number of rows of A. Input, int M, the number of rows of A.
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy EPS. Then a reasonable of A have roughly the same absolute accuracy EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest value for TOL is roughly EPS divided by the magnitude of the largest
element. element.
Output, int *KR, the numerical rank. Output, int *KR, the numerical rank.
Input, double B[M], the right hand side of the linear system. Input, double B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the linear Output, double X[N], a least squares solution to the linear
system. system.
Output, double RSD[M], the residual, B - A*X. RSD may Output, double RSD[M], the residual, B - A*X. RSD may
overwrite B. overwrite B.
Workspace, int JPVT[N], required if ITASK = 1. Workspace, int JPVT[N], required if ITASK = 1.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns independent to within the tolerance TOL and the remaining columns
are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns, of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL. and of R. This estimate will be <= 1/TOL.
Workspace, double QRAUX[N], required if ITASK = 1. Workspace, double QRAUX[N], required if ITASK = 1.
Input, int ITASK. Input, int ITASK.
1, DQRLS factors the matrix A and solves the least squares problem. 1, DQRLS factors the matrix A and solves the least squares problem.
2, DQRLS assumes that the matrix A was factored with an earlier 2, DQRLS assumes that the matrix A was factored with an earlier
call to DQRLS, and only solves the least squares problem. call to DQRLS, and only solves the least squares problem.
Output, int DQRLS, error code. Output, int DQRLS, error code.
0: no error 0: no error
-1: LDA < M (fatal error) -1: LDA < M (fatal error)
-2: N < 1 (fatal error) -2: N < 1 (fatal error)
-3: ITASK < 1 (fatal error) -3: ITASK < 1 (fatal error)
*/ */
{ {
int ind; int ind;
if (lda < m) { if (lda < m) {
/*fprintf ( stderr, "\n" ); /*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" ); fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " LDA < M.\n" );*/ fprintf ( stderr, " LDA < M.\n" );*/
ind = -1; ind = -1;
return ind; return ind;
} }
if (n <= 0) { if (n <= 0) {
/*fprintf ( stderr, "\n" ); /*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" ); fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " N <= 0.\n" );*/ fprintf ( stderr, " N <= 0.\n" );*/
ind = -2; ind = -2;
return ind; return ind;
} }
if (itask < 1) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " ITASK < 1.\n" );*/
ind = -3;
return ind;
}
if (itask < 1) { ind = 0;
/*fprintf ( stderr, "\n" ); /*
fprintf ( stderr, "DQRLS - Fatal error!\n" ); Factor the matrix.
fprintf ( stderr, " ITASK < 1.\n" );*/ */
ind = -3; if (itask == 1)
dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
/*
Solve the least-squares problem.
*/
dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
return ind; return ind;
} }
/******************************************************************************/
ind = 0; void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
/* double rsd[], int jpvt[], double qraux[])
Factor the matrix.
*/
if (itask == 1)
dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
/*
Solve the least-squares problem.
*/
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[],
double rsd[], int jpvt[], double qraux[])
/******************************************************************************/
/*
Purpose:
DQRLSS solves a linear system in a least squares sense. /******************************************************************************/
/*
Purpose:
Discussion: DQRLSS solves a linear system in a least squares sense.
DQRLSS must be preceded by a call to DQRANK. Discussion:
The system is to be solved is DQRLSS must be preceded by a call to DQRANK.
A * X = B
where
A is an M by N matrix with rank KR, as determined by DQRANK,
B is a given M-vector,
X is the N-vector to be computed.
A solution X, with at most KR nonzero components, is found which The system is to be solved is
minimizes the 2-norm of the residual (A*X-B). A * X = B
where
A is an M by N matrix with rank KR, as determined by DQRANK,
B is a given M-vector,
X is the N-vector to be computed.
Once the matrix A has been formed, DQRANK should be A solution X, with at most KR nonzero components, is found which
called once to decompose it. Then, for each right hand minimizes the 2-norm of the residual (A*X-B).
side B, DQRLSS should be called once to obtain the
solution and residual.
Licensing: Once the matrix A has been formed, DQRANK should be
called once to decompose it. Then, for each right hand
side B, DQRLSS should be called once to obtain the
solution and residual.
This code is distributed under the GNU LGPL license. Licensing:
Modified: This code is distributed under the GNU LGPL license.
10 September 2012 Modified:
Author: 10 September 2012
C version by John Burkardt Author:
Parameters: C version by John Burkardt
Input, double A[LDA*N], the QR factorization information Parameters:
from DQRANK. The triangular matrix R of the QR factorization is
contained in the upper triangle and information needed to recover
the orthogonal matrix Q is stored below the diagonal in A and in
the vector QRAUX.
Input, int LDA, the leading dimension of A, which must Input, double A[LDA*N], the QR factorization information
be at least M. from DQRANK. The triangular matrix R of the QR factorization is
contained in the upper triangle and information needed to recover
the orthogonal matrix Q is stored below the diagonal in A and in
the vector QRAUX.
Input, int M, the number of rows of A. Input, int LDA, the leading dimension of A, which must
be at least M.
Input, int N, the number of columns of A. Input, int M, the number of rows of A.
Input, int KR, the rank of the matrix, as estimated by DQRANK. Input, int N, the number of columns of A.
Input, double B[M], the right hand side of the linear system. Input, int KR, the rank of the matrix, as estimated by DQRANK.
Output, double X[N], a least squares solution to the Input, double B[M], the right hand side of the linear system.
linear system.
Output, double RSD[M], the residual, B - A*X. RSD may Output, double X[N], a least squares solution to the
overwrite B. linear system.
Input, int JPVT[N], the pivot information from DQRANK. Output, double RSD[M], the residual, B - A*X. RSD may
Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly overwrite B.
independent to within the tolerance TOL and the remaining columns
are linearly dependent.
Input, double QRAUX[N], auxiliary information from DQRANK Input, int JPVT[N], the pivot information from DQRANK.
defining the QR factorization. Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
*/ independent to within the tolerance TOL and the remaining columns
{ are linearly dependent.
int i;
int info;
int j;
int job;
int k;
double t;
if (kr != 0) { Input, double QRAUX[N], auxiliary information from DQRANK
job = 110; defining the QR factorization.
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job); */
UNUSED(info); {
} int i;
int info;
int j;
int job;
int k;
double t;
if (kr != 0) {
job = 110;
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job);
UNUSED(info);
}
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
jpvt[i] = - jpvt[i]; jpvt[i] = - jpvt[i];
for (i = kr; i < n; i++) for (i = kr; i < n; i++)
x[i] = 0.0; x[i] = 0.0;
for (j = 1; j <= n; j++) { for (j = 1; j <= n; j++) {
if (jpvt[j - 1] <= 0) { if (jpvt[j - 1] <= 0) {
k = - jpvt[j - 1]; k = - jpvt[j - 1];
jpvt[j - 1] = k; jpvt[j - 1] = k;
while (k != j) { while (k != j) {
t = x[j - 1]; t = x[j - 1];
x[j - 1] = x[k - 1]; x[j - 1] = x[k - 1];
x[k - 1] = t; x[k - 1] = t;
jpvt[k - 1] = -jpvt[k - 1]; jpvt[k - 1] = -jpvt[k - 1];
k = jpvt[k - 1]; k = jpvt[k - 1];
}
} }
} }
} }
} /******************************************************************************/
/******************************************************************************/
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) double qy[], double qty[], double b[], double rsd[], double ab[], int job)
/******************************************************************************/ /******************************************************************************/
/* /*
Purpose: Purpose:
DQRSL computes transformations, projections, and least squares solutions. DQRSL computes transformations, projections, and least squares solutions.
Discussion: Discussion:
DQRSL requires the output of DQRDC. DQRSL requires the output of DQRDC.
For K <= min(N,P), let AK be the matrix For K <= min(N,P), let AK be the matrix
AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) ) AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
formed from columns JPVT[0], ..., JPVT(K) of the original formed from columns JPVT[0], ..., JPVT(K) of the original
N by P matrix A that was input to DQRDC. If no pivoting was N by P matrix A that was input to DQRDC. If no pivoting was
done, AK consists of the first K columns of A in their done, AK consists of the first K columns of A in their
original order. DQRDC produces a factored orthogonal matrix Q original order. DQRDC produces a factored orthogonal matrix Q
and an upper triangular matrix R such that and an upper triangular matrix R such that
AK = Q * (R) AK = Q * (R)
(0) (0)
This information is contained in coded form in the arrays This information is contained in coded form in the arrays
A and QRAUX. A and QRAUX.
The parameters QY, QTY, B, RSD, and AB are not referenced The parameters QY, QTY, B, RSD, and AB are not referenced
if their computation is not requested and in this case if their computation is not requested and in this case
can be replaced by dummy variables in the calling program. can be replaced by dummy variables in the calling program.
To save storage, the user may in some cases use the same To save storage, the user may in some cases use the same
array for different parameters in the calling sequence. A array for different parameters in the calling sequence. A
frequently occurring example is when one wishes to compute frequently occurring example is when one wishes to compute
any of B, RSD, or AB and does not need Y or QTY. In this any of B, RSD, or AB and does not need Y or QTY. In this
case one may identify Y, QTY, and one of B, RSD, or AB, while case one may identify Y, QTY, and one of B, RSD, or AB, while
providing separate arrays for anything else that is to be providing separate arrays for anything else that is to be
computed. computed.
Thus the calling sequence Thus the calling sequence
dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info ) dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
will result in the computation of B and RSD, with RSD will result in the computation of B and RSD, with RSD
overwriting Y. More generally, each item in the following overwriting Y. More generally, each item in the following
list contains groups of permissible identifications for list contains groups of permissible identifications for
a single calling sequence. a single calling sequence.
1. (Y,QTY,B) (RSD) (AB) (QY) 1. (Y,QTY,B) (RSD) (AB) (QY)
2. (Y,QTY,RSD) (B) (AB) (QY) 2. (Y,QTY,RSD) (B) (AB) (QY)
3. (Y,QTY,AB) (B) (RSD) (QY) 3. (Y,QTY,AB) (B) (RSD) (QY)
4. (Y,QY) (QTY,B) (RSD) (AB) 4. (Y,QY) (QTY,B) (RSD) (AB)
5. (Y,QY) (QTY,RSD) (B) (AB) 5. (Y,QY) (QTY,RSD) (B) (AB)
6. (Y,QY) (QTY,AB) (B) (RSD) 6. (Y,QY) (QTY,AB) (B) (RSD)
In any group the value returned in the array allocated to In any group the value returned in the array allocated to
the group corresponds to the last member of the group. the group corresponds to the last member of the group.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
07 June 2005 07 June 2005
Author: Author:
C version by John Burkardt. C version by John Burkardt.
Reference: Reference:
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart, Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide, LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics), SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center, 3600 University City Science Center,
Philadelphia, PA, 19104-2688. Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X ISBN 0-89871-172-X
Parameters: Parameters:
Input, double A[LDA*P], contains the output of DQRDC. Input, double A[LDA*P], contains the output of DQRDC.
Input, int LDA, the leading dimension of the array A. Input, int LDA, the leading dimension of the array A.
Input, int N, the number of rows of the matrix AK. It must Input, int N, the number of rows of the matrix AK. It must
have the same value as N in DQRDC. have the same value as N in DQRDC.
Input, int K, the number of columns of the matrix AK. K Input, int K, the number of columns of the matrix AK. K
must not be greater than min(N,P), where P is the same as in the must not be greater than min(N,P), where P is the same as in the
calling sequence to DQRDC. calling sequence to DQRDC.
Input, double QRAUX[P], the auxiliary output from DQRDC. Input, double QRAUX[P], the auxiliary output from DQRDC.
Input, double Y[N], a vector to be manipulated by DQRSL. Input, double Y[N], a vector to be manipulated by DQRSL.
Output, double QY[N], contains Q * Y, if requested. Output, double QY[N], contains Q * Y, if requested.
Output, double QTY[N], contains Q' * Y, if requested. Output, double QTY[N], contains Q' * Y, if requested.
Output, double B[K], the solution of the least squares problem Output, double B[K], the solution of the least squares problem
minimize norm2 ( Y - AK * B), minimize norm2 ( Y - AK * B),
if its computation has been requested. Note that if pivoting was if its computation has been requested. Note that if pivoting was
requested in DQRDC, the J-th component of B will be associated with requested in DQRDC, the J-th component of B will be associated with
column JPVT(J) of the original matrix A that was input into DQRDC. column JPVT(J) of the original matrix A that was input into DQRDC.
Output, double RSD[N], the least squares residual Y - AK * B, Output, double RSD[N], the least squares residual Y - AK * B,
if its computation has been requested. RSD is also the orthogonal if its computation has been requested. RSD is also the orthogonal
projection of Y onto the orthogonal complement of the column space projection of Y onto the orthogonal complement of the column space
of AK. of AK.
Output, double AB[N], the least squares approximation Ak * B, Output, double AB[N], the least squares approximation Ak * B,
if its computation has been requested. AB is also the orthogonal if its computation has been requested. AB is also the orthogonal
projection of Y onto the column space of A. projection of Y onto the column space of A.
Input, integer JOB, specifies what is to be computed. JOB has Input, integer JOB, specifies what is to be computed. JOB has
the decimal expansion ABCDE, with the following meaning: the decimal expansion ABCDE, with the following meaning:
if A != 0, compute QY. if A != 0, compute QY.
if B != 0, compute QTY. if B != 0, compute QTY.
if C != 0, compute QTY and B. if C != 0, compute QTY and B.
if D != 0, compute QTY and RSD. if D != 0, compute QTY and RSD.
if E != 0, compute QTY and AB. if E != 0, compute QTY and AB.
Note that a request to compute B, RSD, or AB automatically triggers Note that a request to compute B, RSD, or AB automatically triggers
the computation of QTY, for which an array must be provided in the the computation of QTY, for which an array must be provided in the
calling sequence. calling sequence.
Output, int DQRSL, is zero unless the computation of B has Output, int DQRSL, is zero unless the computation of B has
been requested and R is exactly singular. In this case, INFO is the 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. index of the first zero diagonal element of R, and B is left unaltered.
*/
{
int cab;
int cb;
int cqty;
int cqy;
int cr;
int i;
int info;
int j;
int jj;
int ju;
double t;
double temp;
/*
Set INFO flag.
*/ */
info = 0; {
int cab;
int cb;
int cqty;
int cqy;
int cr;
int i;
int info;
int j;
int jj;
int ju;
double t;
double temp;
/*
Set INFO flag.
*/
info = 0;
/* /*
Determine what is to be computed. Determine what is to be computed.
*/ */
cqy = ( job / 10000 != 0); cqy = ( job / 10000 != 0);
cqty = ((job % 10000) != 0); cqty = ((job % 10000) != 0);
cb = ((job % 1000) / 100 != 0); cb = ((job % 1000) / 100 != 0);
cr = ((job % 100) / 10 != 0); cr = ((job % 100) / 10 != 0);
cab = ((job % 10) != 0); cab = ((job % 10) != 0);
ju = i4_min(k, n - 1); ju = i4_min(k, n - 1);
/* /*
Special action when N = 1. Special action when N = 1.
*/ */
if (ju == 0) { if (ju == 0) {
if (cqy) if (cqy)
qy[0] = y[0]; qy[0] = y[0];
if (cqty) if (cqty)
qty[0] = y[0]; qty[0] = y[0];
if (cab) if (cab)
ab[0] = y[0]; ab[0] = y[0];
if (cb) { if (cb) {
if (a[0 + 0 * lda] == 0.0) if (a[0 + 0 * lda] == 0.0)
info = 1; info = 1;
else else
b[0] = y[0] / a[0 + 0 * lda]; b[0] = y[0] / a[0 + 0 * lda];
}
if (cr)
rsd[0] = 0.0;
return info;
}
/*
Set up to compute QY or QTY.
*/
if (cqy) {
for (i = 1; i <= n; i++)
qy[i - 1] = y[i - 1];
}
if (cqty) {
for (i = 1; i <= n; i++)
qty[i - 1] = y[i - 1];
}
/*
Compute QY.
*/
if (cqy) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
} }
if (cr)
rsd[0] = 0.0;
return info;
} }
} /*
/* Set up to compute QY or QTY.
Compute Q'*Y. */
*/ if (cqy) {
if (cqty) { for (i = 1; i <= n; i++)
for (j = 1; j <= ju; j++) { qy[i - 1] = y[i - 1];
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
} }
} if (cqty) {
/* for (i = 1; i <= n; i++)
Set up to compute B, RSD, or AB. qty[i - 1] = y[i - 1];
*/ }
if (cb) { /*
for (i = 1; i <= k; i++) Compute QY.
b[i - 1] = qty[i - 1]; */
} if (cqy) {
if (cab) { for (jj = 1; jj <= ju; jj++) {
for (i = 1; i <= k; i++) j = ju - jj + 1;
ab[i - 1] = qty[i - 1]; if (qraux[j - 1] != 0.0) {
} temp = a[j - 1 + (j - 1) * lda];
if (cr && k < n) { a[j - 1 + (j - 1)*lda] = qraux[j - 1];
for (i = k + 1; i <= n; i++) t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda];
rsd[i - 1] = qty[i - 1]; daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1);
} a[j - 1 + (j - 1)*lda] = temp;
if (cab && k + 1 <= n) { }
for (i = k + 1; i <= n; i++)
ab[i - 1] = 0.0;
}
if (cr) {
for (i = 1; i <= k; i++)
rsd[i - 1] = 0.0;
}
/*
Compute B.
*/
if (cb) {
for (jj = 1; jj <= k; jj++) {
j = k - jj + 1;
if (a[j - 1 + (j - 1)*lda] == 0.0) {
info = j;
break;
} }
b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda]; }
if (j != 1) { /*
t = -b[j - 1]; Compute Q'*Y.
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1); */
if (cqty) {
for (j = 1; j <= ju; j++) {
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
} }
} }
} /*
/* Set up to compute B, RSD, or AB.
Compute RSD or AB as required. */
*/ if (cb) {
if (cr || cab) { for (i = 1; i <= k; i++)
for (jj = 1; jj <= ju; jj++) { b[i - 1] = qty[i - 1];
j = ju - jj + 1; }
if (qraux[j - 1] != 0.0) { if (cab) {
temp = a[j - 1 + (j - 1) * lda]; for (i = 1; i <= k; i++)
a[j - 1 + (j - 1)*lda] = qraux[j - 1]; ab[i - 1] = qty[i - 1];
if (cr) { }
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1) if (cr && k < n) {
/ a[j - 1 + (j - 1) * lda]; for (i = k + 1; i <= n; i++)
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1); rsd[i - 1] = qty[i - 1];
}
if (cab && k + 1 <= n) {
for (i = k + 1; i <= n; i++)
ab[i - 1] = 0.0;
}
if (cr) {
for (i = 1; i <= k; i++)
rsd[i - 1] = 0.0;
}
/*
Compute B.
*/
if (cb) {
for (jj = 1; jj <= k; jj++) {
j = k - jj + 1;
if (a[j - 1 + (j - 1)*lda] == 0.0) {
info = j;
break;
}
b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda];
if (j != 1) {
t = -b[j - 1];
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1);
} }
if (cab) { }
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1) }
/ a[j - 1 + (j - 1) * lda]; /*
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1); Compute RSD or AB as required.
*/
if (cr || cab) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
if (cr) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1);
}
if (cab) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1);
}
a[j - 1 + (j - 1)*lda] = temp;
} }
a[j - 1 + (j - 1)*lda] = temp;
} }
} }
return info;
} }
return info; /******************************************************************************/
}
/******************************************************************************/
/******************************************************************************/
void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/ /******************************************************************************/
/*
Purpose:
DSCAL scales a vector by a constant. void dscal(int n, double sa, double x[], int incx)
Licensing: /******************************************************************************/
/*
Purpose:
This code is distributed under the GNU LGPL license. DSCAL scales a vector by a constant.
Modified: Licensing:
30 March 2007 This code is distributed under the GNU LGPL license.
Author: Modified:
C version by John Burkardt 30 March 2007
Reference: Author:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, C version by John Burkardt
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, Reference:
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters: Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Input, int N, the number of entries in the vector. Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Input, double SA, the multiplier. Parameters:
Input/output, double X[*], the vector to be scaled. Input, int N, the number of entries in the vector.
Input, int INCX, the increment between successive entries of X. Input, double SA, the multiplier.
*/
{
int i;
int ix;
int m;
if (n <= 0) return; Input/output, double X[*], the vector to be scaled.
if (incx == 1) { Input, int INCX, the increment between successive entries of X.
m = n % 5; */
for (i = 0; i < m; i++) {
x[i] = sa * x[i]; int i;
for (i = m; i < n; i = i + 5) { int ix;
x[i] = sa * x[i]; int m;
x[i + 1] = sa * x[i + 1];
x[i + 2] = sa * x[i + 2]; if (n <= 0) return;
x[i + 3] = sa * x[i + 3];
x[i + 4] = sa * x[i + 4]; if (incx == 1) {
m = n % 5;
for (i = 0; i < m; i++)
x[i] = sa * x[i];
for (i = m; i < n; i = i + 5) {
x[i] = sa * x[i];
x[i + 1] = sa * x[i + 1];
x[i + 2] = sa * x[i + 2];
x[i + 3] = sa * x[i + 3];
x[i + 4] = sa * x[i + 4];
}
} }
} else {
else { if (0 <= incx)
if (0 <= incx) ix = 0;
ix = 0; else
else ix = (- n + 1) * incx;
ix = (- n + 1) * incx; for (i = 0; i < n; i++) {
for (i = 0; i < n; i++) { x[ix] = sa * x[ix];
x[ix] = sa * x[ix]; ix = ix + 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: Purpose:
DSWAP interchanges two vectors.
Licensing:
This code is distributed under the GNU LGPL license. DSWAP interchanges two vectors.
Modified: Licensing:
30 March 2007 This code is distributed under the GNU LGPL license.
Author: Modified:
C version by John Burkardt 30 March 2007
Reference: Author:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart, C version by John Burkardt
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh, Reference:
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters: Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Input, int N, the number of entries in the vectors. Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Input/output, double X[*], one of the vectors to swap. Parameters:
Input, int INCX, the increment between successive entries of X. Input, int N, the number of entries in the vectors.
Input/output, double Y[*], one of the vectors to swap. Input/output, double X[*], one of the vectors to swap.
Input, int INCY, the increment between successive elements of Y. Input, int INCX, the increment between successive entries of X.
*/
{
if (n <= 0) return;
int i, ix, iy, m; Input/output, double Y[*], one of the vectors to swap.
double temp;
if (incx == 1 && incy == 1) { Input, int INCY, the increment between successive elements of Y.
m = n % 3; */
for (i = 0; i < m; i++) { {
temp = x[i]; if (n <= 0) return;
x[i] = y[i];
y[i] = temp; int i, ix, iy, m;
} double temp;
for (i = m; i < n; i = i + 3) {
temp = x[i]; if (incx == 1 && incy == 1) {
x[i] = y[i]; m = n % 3;
y[i] = temp; for (i = 0; i < m; i++) {
temp = x[i + 1]; temp = x[i];
x[i + 1] = y[i + 1]; x[i] = y[i];
y[i + 1] = temp; y[i] = temp;
temp = x[i + 2]; }
x[i + 2] = y[i + 2]; for (i = m; i < n; i = i + 3) {
y[i + 2] = temp; temp = x[i];
x[i] = y[i];
y[i] = temp;
temp = x[i + 1];
x[i + 1] = y[i + 1];
y[i + 1] = temp;
temp = x[i + 2];
x[i + 2] = y[i + 2];
y[i + 2] = temp;
}
} }
} else {
else { ix = (incx >= 0) ? 0 : (-n + 1) * incx;
ix = (incx >= 0) ? 0 : (-n + 1) * incx; iy = (incy >= 0) ? 0 : (-n + 1) * incy;
iy = (incy >= 0) ? 0 : (-n + 1) * incy; for (i = 0; i < n; i++) {
for (i = 0; i < n; i++) { temp = x[ix];
temp = x[ix]; x[ix] = y[iy];
x[ix] = y[iy]; y[iy] = temp;
y[iy] = temp; ix = ix + incx;
ix = ix + incx; iy = iy + 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: Purpose:
QR_SOLVE solves a linear system in the least squares sense. QR_SOLVE solves a linear system in the least squares sense.
Discussion: Discussion:
If the matrix A has full column rank, then the solution X should be the If the matrix A has full column rank, then the solution X should be the
unique vector that minimizes the Euclidean norm of the residual. unique vector that minimizes the Euclidean norm of the residual.
If the matrix A does not have full column rank, then the solution is If the matrix A does not have full column rank, then the solution is
not unique; the vector X will minimize the residual norm, but so will not unique; the vector X will minimize the residual norm, but so will
various other vectors. various other vectors.
Licensing: Licensing:
This code is distributed under the GNU LGPL license. This code is distributed under the GNU LGPL license.
Modified: Modified:
11 September 2012 11 September 2012
Author: Author:
John Burkardt John Burkardt
Reference: Reference:
David Kahaner, Cleve Moler, Steven Nash, David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software, Numerical Methods and Software,
Prentice Hall, 1989, Prentice Hall, 1989,
ISBN: 0-13-627258-4, ISBN: 0-13-627258-4,
LC: TA345.K34. LC: TA345.K34.
Parameters: Parameters:
Input, int M, the number of rows of A. Input, int M, the number of rows of A.
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double A[M*N], the matrix. Input, double A[M*N], the matrix.
Input, double B[M], the right hand side. Input, double B[M], the right hand side.
Output, double QR_SOLVE[N], the least squares solution. Output, double QR_SOLVE[N], the least squares solution.
*/ */
{ {
double a_qr[n * m], qraux[n], r[m], tol; double a_qr[n * m], qraux[n], r[m], tol;
int ind, itask, jpvt[n], kr, lda; int ind, itask, jpvt[n], kr, lda;
r8mat_copy(a_qr, m, n, a); r8mat_copy(a_qr, m, n, a);
lda = m; lda = m;
tol = r8_epsilon() / r8mat_amax(m, n, a_qr); tol = r8_epsilon() / r8mat_amax(m, n, a_qr);
itask = 1; itask = 1;
ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind); ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind);
} }
/******************************************************************************/ /******************************************************************************/
#endif #endif
MK/module/motion/qr_solve.h
View file @ b3cca544
#if ENABLED(AUTO_BED_LEVELING_GRID) #if ENABLED(AUTO_BED_LEVELING_GRID)
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);
double ddot(int n, 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); double dnrm2(int n, double x[], int incx);
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[]); 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); 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); 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[]); 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); double qy[], double qty[], double b[], double rsd[], double ab[], int job);
void dscal(int n, double sa, double x[], int incx); void dscal(int n, double sa, double x[], int 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);
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[]);
#endif #endif
MK/module/motion/vector_3.cpp
View file @ b3cca544
/* /**
vector_3.cpp - Vector library for bed leveling * vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved. * 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
*/
This library is free software; you can redistribute it and/or #include "../../base.h"
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, #if ENABLED(AUTO_BED_LEVELING_FEATURE)
but WITHOUT ANY WARRANTY; without even the implied warranty of #include <math.h>
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU #include "vector_3.h"
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public vector_3::vector_3() : x(0), y(0), z(0) { }
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" vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
#include <math.h>
#if ENABLED(AUTO_BED_LEVELING_FEATURE) vector_3 vector_3::cross(vector_3 left, vector_3 right) {
#include "vector_3.h" return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
vector_3::vector_3() : x(0), y(0), z(0) { } left.x * right.y - left.y * right.x);
}
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(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::cross(vector_3 left, vector_3 right) { vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z, vector_3 vector_3::get_normal() {
left.x * right.y - left.y * right.x); vector_3 normalized = vector_3(x, y, z);
} normalized.normalize();
return normalized;
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)); }
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
vector_3 vector_3::get_normal() {
vector_3 normalized = vector_3(x, y, z); void vector_3::normalize() {
normalized.normalize(); float length = get_length();
return normalized; x /= length;
} y /= length;
z /= length;
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); } }
void vector_3::normalize() { void vector_3::apply_rotation(matrix_3x3 matrix) {
float length = get_length(); float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
x /= length; float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
y /= length; float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
z /= length; x = resultX;
} y = resultY;
z = resultZ;
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]; void vector_3::debug(const char title[]) {
float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2]; ECHO_ST(DB, title);
x = resultX; ECHO_MV(" x: ", x, 6);
y = resultY; ECHO_MV(" y: ", y, 6);
z = resultZ; ECHO_EMV(" z: ", z, 6);
} }
void vector_3::debug(const char title[]) { void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
ECHO_ST(DB, title); vector_3 vector = vector_3(x, y, z);
ECHO_MV(" x: ", x, 6); vector.apply_rotation(matrix);
ECHO_MV(" y: ", y, 6); x = vector.x;
ECHO_EMV(" z: ", z, 6); y = vector.y;
} z = vector.z;
}
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
vector_3 vector = vector_3(x, y, z); matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
vector.apply_rotation(matrix); //row_0.debug("row_0");
x = vector.x; //row_1.debug("row_1");
y = vector.y; //row_2.debug("row_2");
z = vector.z; matrix_3x3 new_matrix;
} new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) { new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
//row_0.debug("row_0"); //new_matrix.debug("new_matrix");
//row_1.debug("row_1"); return new_matrix;
//row_2.debug("row_2"); }
matrix_3x3 new_matrix;
new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z; void matrix_3x3::set_to_identity() {
new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z; matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z; matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
//new_matrix.debug("new_matrix"); matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
return new_matrix; }
}
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
void matrix_3x3::set_to_identity() { vector_3 z_row = target.get_normal();
matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0; vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0; vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
} // x_row.debug("x_row");
// y_row.debug("y_row");
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) { // z_row.debug("z_row");
vector_3 z_row = target.get_normal();
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal(); // create the matrix already correctly transposed
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
// x_row.debug("x_row"); // rot.debug("rot");
// y_row.debug("y_row"); return rot;
// z_row.debug("z_row"); }
// create the matrix already correctly transposed matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); 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];
// rot.debug("rot"); 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];
return rot; 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;
}
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 new_matrix; void matrix_3x3::debug(const char title[]) {
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]; ECHO_LT(DB, title);
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]; for (uint8_t i = 0; i < 3; i++) {
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]; ECHO_S(DB);
return new_matrix; for (uint8_t j = 0; j < 3; j++) {
} if (matrix[i][j] >= 0.0) ECHO_C('+');
ECHO_V(matrix[i][j], 6);
void matrix_3x3::debug(const char title[]) { ECHO_C(' ');
ECHO_LT(DB, title); }
for (uint8_t i = 0; i < 3; i++) { ECHO_E;
ECHO_S(DB);
for (uint8_t j = 0; j < 3; j++) {
if (matrix[i][j] >= 0.0) ECHO_C('+');
ECHO_V(matrix[i][j], 6);
ECHO_C(' ');
} }
ECHO_E;
} }
}
#endif // AUTO_BED_LEVELING_FEATURE #endif // AUTO_BED_LEVELING_FEATURE
MK/module/motion/vector_3.h
View file @ b3cca544
/* /**
vector_3.cpp - Vector library for bed leveling * vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved. * Copyright (c) 2012 Lars Brubaker. All right reserved.
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * 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, * This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details. * Lesser General Public License for more details.
*
You should have received a copy of the GNU Lesser General Public * You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software * License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/ */
#ifndef VECTOR_3_H #ifndef VECTOR_3_H
#define VECTOR_3_H #define VECTOR_3_H
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
class matrix_3x3;
struct vector_3 {
float x, y, z;
vector_3(); #if ENABLED(AUTO_BED_LEVELING_FEATURE)
vector_3(float x, float y, float z); class matrix_3x3;
static vector_3 cross(vector_3 a, vector_3 b); struct vector_3 {
float x, y, z;
vector_3 operator+(vector_3 v); vector_3();
vector_3 operator-(vector_3 v); vector_3(float x, float y, float z);
void normalize();
float get_length();
vector_3 get_normal();
void debug(const char title[]); static vector_3 cross(vector_3 a, vector_3 b);
void apply_rotation(matrix_3x3 matrix); vector_3 operator+(vector_3 v);
}; vector_3 operator-(vector_3 v);
void normalize();
float get_length();
vector_3 get_normal();
struct matrix_3x3 { void debug(const char title[]);
float matrix[3][3];
static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2); void apply_rotation(matrix_3x3 matrix);
static matrix_3x3 create_look_at(vector_3 target); };
static matrix_3x3 transpose(matrix_3x3 original);
void set_to_identity(); struct matrix_3x3 {
float matrix[3][3];
void debug(const char title[]); static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
}; static matrix_3x3 create_look_at(vector_3 target);
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); void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // AUTO_BED_LEVELING_FEATURE
#endif // AUTO_BED_LEVELING_FEATURE
#endif // VECTOR_3_H #endif // VECTOR_3_H
MK/module/servo/ServoTimers.h
View file @ b3cca544
/* /**
Copyright (c) 2013 Arduino LLC. All right reserved. * Copyright (c) 2013 Arduino LLC. All right reserved.
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * 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, * This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details. * Lesser General Public License for more details.
*
You should have received a copy of the GNU Lesser General Public * You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software * License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/ */
/* /**
* Defines for 16 bit timers used with Servo library * Defines for 16 bit timers used with Servo library
* *
* If _useTimerX is defined then TimerX is a 16 bit timer on the current board * 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; ...@@ -51,4 +51,3 @@ typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t;
#else // everything else #else // everything else
typedef enum { _Nbr_16timers } timer16_Sequence_t; typedef enum { _Nbr_16timers } timer16_Sequence_t;
#endif #endif
MK/module/servo/servo.cpp
View file @ b3cca544
/* /**
Copyright (c) 2013 Arduino LLC. All right reserved. * Copyright (c) 2013 Arduino LLC. All right reserved.
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * 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, * This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details. * Lesser General Public License for more details.
*
You should have received a copy of the GNU Lesser General Public * You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software * License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/ */
#include "../../base.h" #include "../../base.h"
#if HAS(SERVOS) #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 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 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 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 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 // 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_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_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_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(_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_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_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 ) if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{ else{
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*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
}
} }
Channel[timer]++; // increment to the next channel #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { // Interrupt handlers for Arduino
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; #if defined(_useTimer1)
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated SIGNAL (TIMER1_COMPA_vect)
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high {
handle_interrupts(_timer1, &TCNT1, &OCR1A);
} }
else { #endif
// finished all channels so wait for the refresh period to expire before starting over
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*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)
{
handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
#endif
#if defined(_useTimer3)
SIGNAL (TIMER3_COMPA_vect)
{
handle_interrupts(_timer3, &TCNT3, &OCR3A);
}
#endif
#if defined(_useTimer4)
SIGNAL (TIMER4_COMPA_vect)
{
handle_interrupts(_timer4, &TCNT4, &OCR4A);
}
#endif
#if defined(_useTimer5)
SIGNAL (TIMER5_COMPA_vect)
{
handle_interrupts(_timer5, &TCNT5, &OCR5A);
}
#endif
#elif defined WIRING
// Interrupt handlers for Wiring
#if defined(_useTimer1)
void Timer1Service()
{
handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
#endif
#if defined(_useTimer3)
void Timer3Service()
{
handle_interrupts(_timer3, &TCNT3, &OCR3A);
}
#endif
#endif
static void initISR(timer16_Sequence_t timer) #if defined(_useTimer3)
{ SIGNAL (TIMER3_COMPA_vect)
#if defined (_useTimer1) {
if(timer == _timer1) { handle_interrupts(_timer3, &TCNT3, &OCR3A);
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__)
TIFR |= _BV(OCF1A); // clear any pending interrupts;
TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
#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)
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
} }
#endif #endif
#if defined (_useTimer3) #if defined(_useTimer4)
if(timer == _timer3) { SIGNAL (TIMER4_COMPA_vect)
TCCR3A = 0; // normal counting mode {
TCCR3B = _BV(CS31); // set prescaler of 8 handle_interrupts(_timer4, &TCNT4, &OCR4A);
TCNT3 = 0; // clear the timer count
#if defined(__AVR_ATmega128__)
TIFR |= _BV(OCF3A); // clear any pending interrupts;
ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
#else
TIFR3 = _BV(OCF3A); // clear any pending interrupts;
TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
#endif
#if defined(WIRING)
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
#endif
} }
#endif #endif
#if defined (_useTimer4) #if defined(_useTimer5)
if(timer == _timer4) { SIGNAL (TIMER5_COMPA_vect)
TCCR4A = 0; // normal counting mode {
TCCR4B = _BV(CS41); // set prescaler of 8 handle_interrupts(_timer5, &TCNT5, &OCR5A);
TCNT4 = 0; // clear the timer count
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
} }
#endif #endif
#if defined (_useTimer5) #elif defined WIRING
if(timer == _timer5) { // Interrupt handlers for Wiring
TCCR5A = 0; // normal counting mode #if defined(_useTimer1)
TCCR5B = _BV(CS51); // set prescaler of 8 void Timer1Service()
TCNT5 = 0; // clear the timer count {
TIFR5 = _BV(OCF5A); // clear any pending interrupts; handle_interrupts(_timer1, &TCNT1, &OCR1A);
TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
} }
#endif #endif
} #if defined(_useTimer3)
void Timer3Service()
static void finISR(timer16_Sequence_t timer) {
{ handle_interrupts(_timer3, &TCNT3, &OCR3A);
//disable use of the given timer
#if defined WIRING // Wiring
if(timer == _timer1) {
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
#else
TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
#endif
timerDetach(TIMER1OUTCOMPAREA_INT);
} }
else if(timer == _timer3) { #endif
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) #endif
TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
#else
ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt static void initISR(timer16_Sequence_t timer)
#endif {
timerDetach(TIMER3OUTCOMPAREA_INT); #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__)
TIFR |= _BV(OCF1A); // clear any pending interrupts;
TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
#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)
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
}
#endif
#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__)
TIFR |= _BV(OCF3A); // clear any pending interrupts;
ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
#else
TIFR3 = _BV(OCF3A); // clear any pending interrupts;
TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
#endif
#if defined(WIRING)
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
#endif
}
#endif
#if defined (_useTimer4)
if(timer == _timer4) {
TCCR4A = 0; // normal counting mode
TCCR4B = _BV(CS41); // set prescaler of 8
TCNT4 = 0; // clear the timer count
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
}
#endif
#if defined (_useTimer5)
if(timer == _timer5) {
TCCR5A = 0; // normal counting mode
TCCR5B = _BV(CS51); // set prescaler of 8
TCNT5 = 0; // clear the timer count
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
}
#endif
} }
#else
//For arduino - in future: call here to a currently undefined function to reset the timer
#endif
}
static boolean isTimerActive(timer16_Sequence_t timer) static void finISR(timer16_Sequence_t timer)
{ {
// returns true if any servo is active on this timer //disable use of the given timer
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { #if defined WIRING // Wiring
if(SERVO(timer,channel).Pin.isActive == true) if(timer == _timer1) {
return true; #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
#else
TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
#endif
timerDetach(TIMER1OUTCOMPAREA_INT);
}
else if(timer == _timer3) {
#if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
#else
ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
#endif
timerDetach(TIMER3OUTCOMPAREA_INT);
}
#else
//For arduino - in future: call here to a currently undefined function to reset the timer
#endif
} }
return false;
}
/****************** end of static functions ******************************/
Servo::Servo() { static boolean isTimerActive(timer16_Sequence_t timer)
if (ServoCount < MAX_SERVOS) { {
this->servoIndex = ServoCount++; // assign a servo index to this instance // returns true if any servo is active on this timer
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
if(SERVO(timer,channel).Pin.isActive == true)
return true;
}
return false;
} }
else {
this->servoIndex = INVALID_SERVO; // too many servos /****************** end of static functions ******************************/
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
}
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); 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; if (this->servoIndex >= MAX_SERVOS) return -1;
if (pin > 0) servos[this->servoIndex].Pin.nbr = pin; if (pin > 0) servos[this->servoIndex].Pin.nbr = pin;
pinMode(servos[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output pinMode(servos[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max) / 4; this->max = (MAX_PULSE_WIDTH - max) / 4;
// initialize the timer if it has not already been initialized // initialize the timer if it has not already been initialized
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) initISR(timer); if (!isTimerActive(timer)) initISR(timer);
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
return this->servoIndex; return this->servoIndex;
} }
void Servo::detach() { void Servo::detach() {
servos[this->servoIndex].Pin.isActive = false; servos[this->servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) finISR(timer); 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) 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()); value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
}
this->writeMicroseconds(value);
} }
this->writeMicroseconds(value);
} void Servo::writeMicroseconds(int value) {
// calculate and store the values for the given channel
void Servo::writeMicroseconds(int value) { byte channel = this->servoIndex;
// calculate and store the values for the given channel if (channel < MAX_SERVOS) { // ensure channel is valid
byte channel = this->servoIndex; // ensure pulse width is valid
if (channel < MAX_SERVOS) { // ensure channel is valid value = constrain(value, SERVO_MIN(), SERVO_MAX()) - TRIM_DURATION;
// ensure pulse width is valid value = usToTicks(value); // convert to ticks after compensating for interrupt overhead
value = constrain(value, SERVO_MIN(), SERVO_MAX()) - TRIM_DURATION;
value = usToTicks(value); // convert to ticks after compensating for interrupt overhead CRITICAL_SECTION_START;
servos[channel].ticks = value;
CRITICAL_SECTION_START; CRITICAL_SECTION_END;
servo_info[channel].ticks = value; }
CRITICAL_SECTION_END;
} }
}
// return the value as degrees // return the value as degrees
int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); } 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; 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) { if (this->attach(0) >= 0) {
this->write(value); this->write(value);
#if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE) #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
delay(SERVO_DEACTIVATION_DELAY); delay(SERVO_DEACTIVATION_DELAY);
this->detach(); this->detach();
#endif #endif
}
} }
}
#endif #endif
MK/module/servo/servo.h
View file @ b3cca544
...@@ -45,63 +45,63 @@ ...@@ -45,63 +45,63 @@
detach() - Stops an attached servos from pulsing its i/o pin. detach() - Stops an attached servos from pulsing its i/o pin.
*/ */
#ifndef SERVO_H #ifndef _SERVO_H
#define 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 * Defines for 16 bit timers used with Servo library
* timer16_Sequence_t enumerates the sequence that the timers should be allocated *
* _Nbr_16timers indicates how many 16 bit timers are available. * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
*/ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
* _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 MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo #define MAX_PULSE_WIDTH 2400 // the longest 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 DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds
#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 SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer #define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_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 nbr :6 ; // a pin number from 0 to 63 uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false
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;
ServoPin_t Pin; volatile unsigned int ticks;
volatile unsigned int ticks; } servo_t;
} servo_t;
class Servo {
class Servo { public:
public: Servo();
Servo(); uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. void detach();
void detach(); void write(int value); // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
void write(int value); // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds void writeMicroseconds(int value); // Write pulse width in microseconds
void writeMicroseconds(int value); // Write pulse width in microseconds void move(int value); // attach the servo, then move to value
void move(int value); // attach the servo, then move to value // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
// if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DEACTIVATION_DELAY, then detach
// if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DEACTIVATION_DELAY, then detach int read(); // returns current pulse width as an angle between 0 and 180 degrees
int read(); // returns current pulse width as an angle between 0 and 180 degrees int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) bool attached(); // return true if this servo is attached, otherwise false
bool attached(); // return true if this servo is attached, otherwise false
private:
private: uint8_t servoIndex; // index into the channel data for this 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 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
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH };
};
#endif // SERVOS
#endif #endif // _SERVO_H
MK/module/watchdog/watchdog.cpp
View file @ b3cca544
#include "../../base.h" #include "../../base.h"
#if ENABLED(USE_WATCHDOG) #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 #if ENABLED(WATCHDOG_RESET_MANUAL)
void watchdog_init() { // We enable the watchdog timer, but only for the interrupt.
#if ENABLED(WATCHDOG_RESET_MANUAL) // Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details.
// We enable the watchdog timer, but only for the interrupt. wdt_reset();
// Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details. _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
wdt_reset(); _WD_CONTROL_REG = _BV(WDIE) | WDTO_4S;
_WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE); #else
_WD_CONTROL_REG = _BV(WDIE) | WDTO_4S; wdt_enable(WDTO_4S);
#else #endif
wdt_enable(WDTO_4S); }
#endif
}
//=========================================================================== //===========================================================================
//=================================== ISR =================================== //=================================== ISR ===================================
//=========================================================================== //===========================================================================
// Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled. // Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
#if ENABLED(WATCHDOG_RESET_MANUAL) #if ENABLED(WATCHDOG_RESET_MANUAL)
ISR(WDT_vect) { ISR(WDT_vect) {
ECHO_LM(ER, "Something is wrong, please turn off the printer."); 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 kill(PSTR("ERR:Please Reset")); //kill blocks //16 characters so it fits on a 16x2 display
while (1); //wait for user or serial reset while (1); //wait for user or serial reset
} }
#endif //WATCHDOG_RESET_MANUAL #endif //WATCHDOG_RESET_MANUAL
#endif //USE_WATCHDOG #endif //USE_WATCHDOG
MK/module/watchdog/watchdog.h
View file @ b3cca544
#ifndef WATCHDOG_H #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();
// 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(); }
// 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 #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