Alex Borro
11 years ago
12 changed files with 806 additions and 4 deletions
@ -0,0 +1,52 @@ |
|||
/* -*- c++ -*- */ |
|||
|
|||
/*
|
|||
Reprap firmware based on Sprinter and grbl. |
|||
Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm |
|||
|
|||
This program 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. |
|||
|
|||
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
|||
*/ |
|||
|
|||
/*
|
|||
This firmware is a mashup between Sprinter and grbl. |
|||
(https://github.com/kliment/Sprinter)
|
|||
(https://github.com/simen/grbl/tree)
|
|||
|
|||
It has preliminary support for Matthew Roberts advance algorithm |
|||
http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
|
|||
*/ |
|||
|
|||
/* All the implementation is done in *.cpp files to get better compatibility with avr-gcc without the Arduino IDE */ |
|||
/* Use this file to help the Arduino IDE find which Arduino libraries are needed and to keep documentation on GCode */ |
|||
|
|||
#include "Configuration.h" |
|||
#include "pins.h" |
|||
|
|||
#ifdef ULTRA_LCD |
|||
#if defined(LCD_I2C_TYPE_PCF8575) |
|||
#include <Wire.h> |
|||
#include <LiquidCrystal_I2C.h> |
|||
#elif defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008) |
|||
#include <Wire.h> |
|||
#include <LiquidTWI2.h> |
|||
#elif defined(DOGLCD) |
|||
#include <U8glib.h> // library for graphics LCD by Oli Kraus (https://code.google.com/p/u8glib/) |
|||
#else |
|||
#include <LiquidCrystal.h> // library for character LCD |
|||
#endif |
|||
#endif |
|||
|
|||
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1 |
|||
#include <SPI.h> |
|||
#endif |
|||
@ -0,0 +1,202 @@ |
|||
/*
|
|||
vector_3.cpp - Vector library for bed leveling |
|||
Copyright (c) 2012 Lars Brubaker. All right reserved. |
|||
|
|||
This library is free software; you can redistribute it and/or |
|||
modify it under the terms of the GNU Lesser General Public |
|||
License as published by the Free Software Foundation; either |
|||
version 2.1 of the License, or (at your option) any later version. |
|||
|
|||
This library is distributed in the hope that it will be useful, |
|||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|||
Lesser General Public License for more details. |
|||
|
|||
You should have received a copy of the GNU Lesser General Public |
|||
License along with this library; if not, write to the Free Software |
|||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|||
*/ |
|||
#include <math.h> |
|||
#include "Marlin.h" |
|||
|
|||
#ifdef ENABLE_AUTO_BED_LEVELING |
|||
#include "vector_3.h" |
|||
|
|||
vector_3::vector_3() |
|||
{ |
|||
this->x = 0; |
|||
this->y = 0; |
|||
this->z = 0; |
|||
} |
|||
|
|||
vector_3::vector_3(float x, float y, float z) |
|||
{ |
|||
this->x = x; |
|||
this->y = y; |
|||
this->z = z; |
|||
} |
|||
|
|||
vector_3 vector_3::cross(vector_3 left, vector_3 right) |
|||
{ |
|||
return vector_3(left.y * right.z - left.z * right.y, |
|||
left.z * right.x - left.x * right.z, |
|||
left.x * right.y - left.y * right.x); |
|||
} |
|||
|
|||
vector_3 vector_3::operator+(vector_3 v) |
|||
{ |
|||
return vector_3((x + v.x), (y + v.y), (z + v.z)); |
|||
} |
|||
|
|||
vector_3 vector_3::operator-(vector_3 v) |
|||
{ |
|||
return vector_3((x - v.x), (y - v.y), (z - v.z)); |
|||
} |
|||
|
|||
vector_3 vector_3::get_normal() |
|||
{ |
|||
vector_3 normalized = vector_3(x, y, z); |
|||
normalized.normalize(); |
|||
return normalized; |
|||
} |
|||
|
|||
float vector_3::get_length() |
|||
{ |
|||
float length = sqrt((x * x) + (y * y) + (z * z)); |
|||
return length; |
|||
} |
|||
|
|||
void vector_3::normalize() |
|||
{ |
|||
float length = get_length(); |
|||
x /= length; |
|||
y /= length; |
|||
z /= length; |
|||
} |
|||
|
|||
void vector_3::apply_rotation(matrix_3x3 matrix) |
|||
{ |
|||
float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0]; |
|||
float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1]; |
|||
float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2]; |
|||
|
|||
x = resultX; |
|||
y = resultY; |
|||
z = resultZ; |
|||
} |
|||
|
|||
void vector_3::debug(char* title) |
|||
{ |
|||
SERIAL_PROTOCOL(title); |
|||
SERIAL_PROTOCOLPGM(" x: "); |
|||
SERIAL_PROTOCOL(x); |
|||
SERIAL_PROTOCOLPGM(" y: "); |
|||
SERIAL_PROTOCOL(y); |
|||
SERIAL_PROTOCOLPGM(" z: "); |
|||
SERIAL_PROTOCOL(z); |
|||
SERIAL_PROTOCOLPGM("\n"); |
|||
} |
|||
|
|||
void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z) |
|||
{ |
|||
vector_3 vector = vector_3(x, y, z); |
|||
vector.apply_rotation(matrix); |
|||
x = vector.x; |
|||
y = vector.y; |
|||
z = vector.z; |
|||
} |
|||
|
|||
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) |
|||
{ |
|||
//row_0.debug("row_0");
|
|||
//row_1.debug("row_1");
|
|||
//row_2.debug("row_2");
|
|||
matrix_3x3 new_matrix; |
|||
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z; |
|||
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z; |
|||
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z; |
|||
//new_matrix.debug("new_matrix");
|
|||
|
|||
return new_matrix; |
|||
} |
|||
|
|||
void matrix_3x3::set_to_identity() |
|||
{ |
|||
matrix[0] = 1; matrix[1] = 0; matrix[2] = 0; |
|||
matrix[3] = 0; matrix[4] = 1; matrix[5] = 0; |
|||
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1; |
|||
} |
|||
|
|||
matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up) |
|||
{ |
|||
// There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
|
|||
// through several dot products. The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
|
|||
vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal(); |
|||
vector_3 x_row = vector_3::cross(up, z_row).get_normal(); |
|||
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); |
|||
|
|||
//x_row.debug("x_row");
|
|||
//y_row.debug("y_row");
|
|||
//z_row.debug("z_row");
|
|||
|
|||
matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x), |
|||
vector_3(x_row.y, y_row.y, z_row.y), |
|||
vector_3(x_row.z, y_row.z, z_row.z)); |
|||
|
|||
//rot.debug("rot");
|
|||
return rot; |
|||
} |
|||
|
|||
matrix_3x3 matrix_3x3::create_inverse(matrix_3x3 original) |
|||
{ |
|||
//original.debug("original");
|
|||
float* A = original.matrix; |
|||
float determinant = |
|||
+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) |
|||
- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) |
|||
+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]); |
|||
matrix_3x3 inverse; |
|||
inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant; |
|||
inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant; |
|||
inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant; |
|||
inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant; |
|||
inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant; |
|||
inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant; |
|||
inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant; |
|||
inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant; |
|||
inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant; |
|||
|
|||
vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]); |
|||
vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]); |
|||
vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]); |
|||
|
|||
row0.normalize(); |
|||
row1.normalize(); |
|||
row2.normalize(); |
|||
|
|||
inverse = matrix_3x3::create_from_rows(row0, row1, row2); |
|||
|
|||
//inverse.debug("inverse");
|
|||
return inverse; |
|||
} |
|||
|
|||
void matrix_3x3::debug(char* title) |
|||
{ |
|||
SERIAL_PROTOCOL(title); |
|||
SERIAL_PROTOCOL("\n"); |
|||
int count = 0; |
|||
for(int i=0; i<3; i++) |
|||
{ |
|||
for(int j=0; j<3; j++) |
|||
{ |
|||
SERIAL_PROTOCOL(matrix[count]); |
|||
SERIAL_PROTOCOLPGM(" "); |
|||
count++; |
|||
} |
|||
|
|||
SERIAL_PROTOCOLPGM("\n"); |
|||
} |
|||
} |
|||
|
|||
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
|
|||
|
|||
@ -0,0 +1,62 @@ |
|||
/*
|
|||
vector_3.cpp - Vector library for bed leveling |
|||
Copyright (c) 2012 Lars Brubaker. All right reserved. |
|||
|
|||
This library is free software; you can redistribute it and/or |
|||
modify it under the terms of the GNU Lesser General Public |
|||
License as published by the Free Software Foundation; either |
|||
version 2.1 of the License, or (at your option) any later version. |
|||
|
|||
This library is distributed in the hope that it will be useful, |
|||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|||
Lesser General Public License for more details. |
|||
|
|||
You should have received a copy of the GNU Lesser General Public |
|||
License along with this library; if not, write to the Free Software |
|||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|||
*/ |
|||
#ifndef VECTOR_3_H |
|||
#define VECTOR_3_H |
|||
|
|||
#ifdef ENABLE_AUTO_BED_LEVELING |
|||
class matrix_3x3; |
|||
|
|||
struct vector_3 |
|||
{ |
|||
float x, y, z; |
|||
|
|||
vector_3(); |
|||
vector_3(float x, float y, float z); |
|||
|
|||
static vector_3 cross(vector_3 a, vector_3 b); |
|||
|
|||
vector_3 operator+(vector_3 v); |
|||
vector_3 operator-(vector_3 v); |
|||
void normalize(); |
|||
float get_length(); |
|||
vector_3 get_normal(); |
|||
|
|||
void debug(char* title); |
|||
|
|||
void apply_rotation(matrix_3x3 matrix); |
|||
}; |
|||
|
|||
struct matrix_3x3 |
|||
{ |
|||
float matrix[9]; |
|||
|
|||
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, vector_3 up); |
|||
static matrix_3x3 create_inverse(matrix_3x3 original); |
|||
|
|||
void set_to_identity(); |
|||
|
|||
void debug(char* title); |
|||
}; |
|||
|
|||
|
|||
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float &x, float& y, float& z); |
|||
#endif // ENABLE_AUTO_BED_LEVELING
|
|||
|
|||
#endif // VECTOR_3_H
|
|||
Loading…
Reference in new issue