├── marlin.cat3 ├── BlinkM.h ├── ConfigurationStore.h ├── watchdog.h ├── BlinkM.cpp ├── qr_solve.h ├── motion_control.h ├── SdFatUtil.h ├── SdFile.h ├── digipot_mcp4451.cpp ├── vector_3.h ├── Marlin.ino ├── Ajg_Marlin_v5.ino ├── watchdog.cpp ├── SdFatUtil.cpp ├── cardreader.h ├── AjgHardwareSerial.h ├── SdFile.cpp ├── LiquidCrystalRus.h ├── ultralcd.h ├── ultralcd_st7920_u8glib_rrd.h ├── SdFatConfig.h ├── stepper.h ├── vector_3.cpp ├── temperature.h ├── Servo.h ├── MarlinSerial.h ├── DOGMbitmaps.h ├── motion_control.cpp ├── planner.h ├── MarlinSerial.cpp ├── AjgHardwareSerial.cpp ├── SdVolume.h ├── Sd2Card.h ├── SdInfo.h ├── Marlin.h ├── speed_lookuptable.h ├── LiquidCrystalRus.cpp ├── ConfigurationStore.cpp ├── SdVolume.cpp └── Servo.cpp /marlin.cat3: -------------------------------------------------------------------------------- 1 | #InSight Catalog 2 | 3 | 4 | -------------------------------------------------------------------------------- /BlinkM.h: -------------------------------------------------------------------------------- 1 | /* 2 | BlinkM.h 3 | Library header file for BlinkM library 4 | */ 5 | #if (ARDUINO >= 100) 6 | # include "Arduino.h" 7 | #else 8 | # include "WProgram.h" 9 | #endif 10 | 11 | #include "Wire.h" 12 | 13 | void SendColors(byte red, byte grn, byte blu); 14 | 15 | -------------------------------------------------------------------------------- /ConfigurationStore.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIG_STORE_H 2 | #define CONFIG_STORE_H 3 | 4 | #include "Configuration.h" 5 | 6 | void Config_ResetDefault(); 7 | 8 | #ifndef DISABLE_M503 9 | void Config_PrintSettings(); 10 | #else 11 | FORCE_INLINE void Config_PrintSettings() {} 12 | #endif 13 | 14 | #ifdef EEPROM_SETTINGS 15 | void Config_StoreSettings(); 16 | void Config_RetrieveSettings(); 17 | #else 18 | FORCE_INLINE void Config_StoreSettings() {} 19 | FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); } 20 | #endif 21 | 22 | #endif//CONFIG_STORE_H 23 | -------------------------------------------------------------------------------- /watchdog.h: -------------------------------------------------------------------------------- 1 | #ifndef WATCHDOG_H 2 | #define WATCHDOG_H 3 | 4 | #include "Marlin.h" 5 | 6 | #ifdef USE_WATCHDOG 7 | // initialize watch dog with a 1 sec interrupt time 8 | void watchdog_init(); 9 | // pad the dog/reset watchdog. MUST be called at least every second after the first watchdog_init or AVR will go into emergency procedures.. 10 | void watchdog_reset(); 11 | #else 12 | //If we do not have a watchdog, then we can have empty functions which are optimized away. 13 | FORCE_INLINE void watchdog_init() {}; 14 | FORCE_INLINE void watchdog_reset() {}; 15 | #endif 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /BlinkM.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | BlinkM.cpp - Library for controlling a BlinkM over i2c 3 | Created by Tim Koster, August 21 2013. 4 | */ 5 | #include "Marlin.h" 6 | #ifdef BLINKM 7 | 8 | #if (ARDUINO >= 100) 9 | # include "Arduino.h" 10 | #else 11 | # include "WProgram.h" 12 | #endif 13 | 14 | #include "BlinkM.h" 15 | 16 | void SendColors(byte red, byte grn, byte blu) 17 | { 18 | Wire.begin(); 19 | Wire.beginTransmission(0x09); 20 | Wire.write('o'); //to disable ongoing script, only needs to be used once 21 | Wire.write('n'); 22 | Wire.write(red); 23 | Wire.write(grn); 24 | Wire.write(blu); 25 | Wire.endTransmission(); 26 | } 27 | 28 | #endif //BLINKM 29 | 30 | -------------------------------------------------------------------------------- /qr_solve.h: -------------------------------------------------------------------------------- 1 | #include "Configuration.h" 2 | 3 | #ifdef AUTO_BED_LEVELING_GRID 4 | 5 | void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy ); 6 | double ddot ( int n, double dx[], int incx, double dy[], int incy ); 7 | double dnrm2 ( int n, double x[], int incx ); 8 | void dqrank ( double a[], int lda, int m, int n, double tol, int *kr, 9 | int jpvt[], double qraux[] ); 10 | void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[], 11 | double work[], int job ); 12 | int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[], 13 | double x[], double rsd[], int jpvt[], double qraux[], int itask ); 14 | void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[], 15 | double rsd[], int jpvt[], double qraux[] ); 16 | int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[], 17 | double qy[], double qty[], double b[], double rsd[], double ab[], int job ); 18 | void dscal ( int n, double sa, double x[], int incx ); 19 | void dswap ( int n, double x[], int incx, double y[], int incy ); 20 | double *qr_solve ( int m, int n, double a[], double b[] ); 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /motion_control.h: -------------------------------------------------------------------------------- 1 | /* 2 | motion_control.h - high level interface for issuing motion commands 3 | Part of Grbl 4 | 5 | Copyright (c) 2009-2011 Simen Svale Skogsrud 6 | Copyright (c) 2011 Sungeun K. Jeon 7 | 8 | Grbl is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | Grbl is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with Grbl. If not, see . 20 | */ 21 | 22 | #ifndef motion_control_h 23 | #define motion_control_h 24 | 25 | // Execute an arc in offset mode format. position == current xyz, target == target xyz, 26 | // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is 27 | // the direction of helical travel, radius == circle radius, isclockwise boolean. Used 28 | // for vector transformation direction. 29 | void mc_arc(float *position, float *target, float *offset, unsigned char axis_0, unsigned char axis_1, 30 | unsigned char axis_linear, float feed_rate, float radius, unsigned char isclockwise, uint8_t extruder); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /SdFatUtil.h: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2008 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | #ifdef SDSUPPORT 22 | 23 | #ifndef SdFatUtil_h 24 | #define SdFatUtil_h 25 | /** 26 | * \file 27 | * \brief Useful utility functions. 28 | */ 29 | #include "Marlin.h" 30 | #include "MarlinSerial.h" 31 | /** Store and print a string in flash memory.*/ 32 | #define PgmPrint(x) SerialPrint_P(PSTR(x)) 33 | /** Store and print a string in flash memory followed by a CR/LF.*/ 34 | #define PgmPrintln(x) SerialPrintln_P(PSTR(x)) 35 | 36 | namespace SdFatUtil { 37 | int FreeRam(); 38 | void print_P( PGM_P str); 39 | void println_P( PGM_P str); 40 | void SerialPrint_P(PGM_P str); 41 | void SerialPrintln_P(PGM_P str); 42 | } 43 | 44 | using namespace SdFatUtil; // NOLINT 45 | #endif // #define SdFatUtil_h 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /SdFile.h: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | /** 21 | * \file 22 | * \brief SdFile class 23 | */ 24 | #include "Marlin.h" 25 | 26 | #ifdef SDSUPPORT 27 | #include "SdBaseFile.h" 28 | #include 29 | #ifndef SdFile_h 30 | #define SdFile_h 31 | //------------------------------------------------------------------------------ 32 | /** 33 | * \class SdFile 34 | * \brief SdBaseFile with Print. 35 | */ 36 | class SdFile : public SdBaseFile, public Print { 37 | public: 38 | SdFile() {} 39 | SdFile(const char* name, uint8_t oflag); 40 | #if ARDUINO >= 100 41 | size_t write(uint8_t b); 42 | #else 43 | void write(uint8_t b); 44 | #endif 45 | 46 | int16_t write(const void* buf, uint16_t nbyte); 47 | void write(const char* str); 48 | void write_P(PGM_P str); 49 | void writeln_P(PGM_P str); 50 | }; 51 | #endif // SdFile_h 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /digipot_mcp4451.cpp: -------------------------------------------------------------------------------- 1 | #include "Configuration.h" 2 | 3 | #ifdef DIGIPOT_I2C 4 | #include "Stream.h" 5 | #include "utility/twi.h" 6 | #include "Wire.h" 7 | 8 | // Settings for the I2C based DIGIPOT (MCP4451) on Azteeg X3 Pro 9 | #if MOTHERBOARD == 88 10 | #define DIGIPOT_I2C_FACTOR 117.96 11 | #define DIGIPOT_I2C_MAX_CURRENT 1.736 12 | #else 13 | #define DIGIPOT_I2C_FACTOR 106.7 14 | #define DIGIPOT_I2C_MAX_CURRENT 2.5 15 | #endif 16 | 17 | static byte current_to_wiper( float current ){ 18 | return byte(ceil(float((DIGIPOT_I2C_FACTOR*current)))); 19 | } 20 | 21 | static void i2c_send(byte addr, byte a, byte b) 22 | { 23 | Wire.beginTransmission(addr); 24 | Wire.write(a); 25 | Wire.write(b); 26 | Wire.endTransmission(); 27 | } 28 | 29 | // This is for the MCP4451 I2C based digipot 30 | void digipot_i2c_set_current( int channel, float current ) 31 | { 32 | current = min( (float) max( current, 0.0f ), DIGIPOT_I2C_MAX_CURRENT); 33 | // these addresses are specific to Azteeg X3 Pro, can be set to others, 34 | // In this case first digipot is at address A0=0, A1= 0, second one is at A0=0, A1= 1 35 | byte addr= 0x2C; // channel 0-3 36 | if(channel >= 4) { 37 | addr= 0x2E; // channel 4-7 38 | channel-= 4; 39 | } 40 | 41 | // Initial setup 42 | i2c_send( addr, 0x40, 0xff ); 43 | i2c_send( addr, 0xA0, 0xff ); 44 | 45 | // Set actual wiper value 46 | byte addresses[4] = { 0x00, 0x10, 0x60, 0x70 }; 47 | i2c_send( addr, addresses[channel], current_to_wiper(current) ); 48 | } 49 | 50 | void digipot_i2c_init() 51 | { 52 | const float digipot_motor_current[] = DIGIPOT_I2C_MOTOR_CURRENTS; 53 | Wire.begin(); 54 | // setup initial currents as defined in Configuration_adv.h 55 | for(int i=0;i<=sizeof(digipot_motor_current)/sizeof(float);i++) { 56 | digipot_i2c_set_current(i, digipot_motor_current[i]); 57 | } 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /vector_3.h: -------------------------------------------------------------------------------- 1 | /* 2 | vector_3.cpp - Vector library for bed leveling 3 | Copyright (c) 2012 Lars Brubaker. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | #ifndef VECTOR_3_H 20 | #define VECTOR_3_H 21 | 22 | #ifdef ENABLE_AUTO_BED_LEVELING 23 | class matrix_3x3; 24 | 25 | struct vector_3 26 | { 27 | float x, y, z; 28 | 29 | vector_3(); 30 | vector_3(float x, float y, float z); 31 | 32 | static vector_3 cross(vector_3 a, vector_3 b); 33 | 34 | vector_3 operator+(vector_3 v); 35 | vector_3 operator-(vector_3 v); 36 | void normalize(); 37 | float get_length(); 38 | vector_3 get_normal(); 39 | 40 | void debug(char* title); 41 | 42 | void apply_rotation(matrix_3x3 matrix); 43 | }; 44 | 45 | struct matrix_3x3 46 | { 47 | float matrix[9]; 48 | 49 | static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2); 50 | static matrix_3x3 create_look_at(vector_3 target); 51 | static matrix_3x3 transpose(matrix_3x3 original); 52 | 53 | void set_to_identity(); 54 | 55 | void debug(char* title); 56 | }; 57 | 58 | 59 | void apply_rotation_xyz(matrix_3x3 rotationMatrix, float &x, float& y, float& z); 60 | #endif // ENABLE_AUTO_BED_LEVELING 61 | 62 | #endif // VECTOR_3_H 63 | -------------------------------------------------------------------------------- /Marlin.ino: -------------------------------------------------------------------------------- 1 | /* -*- c++ -*- */ 2 | 3 | /* 4 | Reprap firmware based on Sprinter and grbl. 5 | Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm 6 | 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | /* 22 | This firmware is a mashup between Sprinter and grbl. 23 | (https://github.com/kliment/Sprinter) 24 | (https://github.com/simen/grbl/tree) 25 | 26 | It has preliminary support for Matthew Roberts advance algorithm 27 | http://reprap.org/pipermail/reprap-dev/2011-May/003323.html 28 | */ 29 | 30 | /* All the implementation is done in *.cpp files to get better compatibility with avr-gcc without the Arduino IDE */ 31 | /* Use this file to help the Arduino IDE find which Arduino libraries are needed and to keep documentation on GCode */ 32 | 33 | #include "Configuration.h" 34 | #include "pins.h" 35 | 36 | #ifdef ULTRA_LCD 37 | #if defined(LCD_I2C_TYPE_PCF8575) 38 | #include 39 | #include 40 | #elif defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008) 41 | #include 42 | #include 43 | #elif defined(DOGLCD) 44 | #include // library for graphics LCD by Oli Kraus (https://code.google.com/p/u8glib/) 45 | #else 46 | #include // library for character LCD 47 | #endif 48 | #endif 49 | 50 | #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1 51 | #include 52 | #endif 53 | 54 | #if defined(DIGIPOT_I2C) 55 | #include 56 | #endif 57 | -------------------------------------------------------------------------------- /Ajg_Marlin_v5.ino: -------------------------------------------------------------------------------- 1 | /* -*- c++ -*- */ 2 | 3 | /* 4 | Reprap firmware based on Sprinter and grbl. 5 | Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm 6 | 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | /* 22 | This firmware is a mashup between Sprinter and grbl. 23 | (https://github.com/kliment/Sprinter) 24 | (https://github.com/simen/grbl/tree) 25 | 26 | It has preliminary support for Matthew Roberts advance algorithm 27 | http://reprap.org/pipermail/reprap-dev/2011-May/003323.html 28 | */ 29 | 30 | /* All the implementation is done in *.cpp files to get better compatibility with avr-gcc without the Arduino IDE */ 31 | /* Use this file to help the Arduino IDE find which Arduino libraries are needed and to keep documentation on GCode */ 32 | 33 | #include "Configuration.h" 34 | #include "pins.h" 35 | 36 | #ifdef ULTRA_LCD 37 | #if defined(LCD_I2C_TYPE_PCF8575) 38 | #include 39 | #include 40 | #elif defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008) 41 | #include 42 | #include 43 | #elif defined(DOGLCD) 44 | #include // library for graphics LCD by Oli Kraus (https://code.google.com/p/u8glib/) 45 | #else 46 | #include // library for character LCD 47 | #endif 48 | #endif 49 | 50 | #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1 51 | #include 52 | #endif 53 | 54 | #if defined(DIGIPOT_I2C) 55 | #include 56 | #endif 57 | -------------------------------------------------------------------------------- /watchdog.cpp: -------------------------------------------------------------------------------- 1 | #include "Marlin.h" 2 | 3 | #ifdef USE_WATCHDOG 4 | #include 5 | 6 | #include "watchdog.h" 7 | #include "ultralcd.h" 8 | 9 | //=========================================================================== 10 | //=============================private variables ============================ 11 | //=========================================================================== 12 | 13 | //=========================================================================== 14 | //=============================functinos ============================ 15 | //=========================================================================== 16 | 17 | 18 | /// intialise watch dog with a 4 sec interrupt time 19 | void watchdog_init() 20 | { 21 | #ifdef WATCHDOG_RESET_MANUAL 22 | //We enable the watchdog timer, but only for the interrupt. 23 | //Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details. 24 | wdt_reset(); 25 | _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE); 26 | _WD_CONTROL_REG = _BV(WDIE) | WDTO_4S; 27 | #else 28 | wdt_enable(WDTO_4S); 29 | #endif 30 | } 31 | 32 | /// reset watchdog. MUST be called every 1s after init or avr will reset. 33 | void watchdog_reset() 34 | { 35 | wdt_reset(); 36 | } 37 | 38 | //=========================================================================== 39 | //=============================ISR ============================ 40 | //=========================================================================== 41 | 42 | //Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled. 43 | #ifdef WATCHDOG_RESET_MANUAL 44 | ISR(WDT_vect) 45 | { 46 | //TODO: This message gets overwritten by the kill() call 47 | LCD_ALERTMESSAGEPGM("ERR:Please Reset");//16 characters so it fits on a 16x2 display 48 | lcd_update(); 49 | SERIAL_ERROR_START; 50 | SERIAL_ERRORLNPGM("Something is wrong, please turn off the printer."); 51 | kill(); //kill blocks 52 | while(1); //wait for user or serial reset 53 | } 54 | #endif//RESET_MANUAL 55 | 56 | #endif//USE_WATCHDOG 57 | -------------------------------------------------------------------------------- /SdFatUtil.cpp: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2008 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | 22 | #ifdef SDSUPPORT 23 | #include "SdFatUtil.h" 24 | 25 | //------------------------------------------------------------------------------ 26 | /** Amount of free RAM 27 | * \return The number of free bytes. 28 | */ 29 | int SdFatUtil::FreeRam() { 30 | extern int __bss_end; 31 | extern int* __brkval; 32 | int free_memory; 33 | if (reinterpret_cast(__brkval) == 0) { 34 | // if no heap use from end of bss section 35 | free_memory = reinterpret_cast(&free_memory) 36 | - reinterpret_cast(&__bss_end); 37 | } else { 38 | // use from top of stack to heap 39 | free_memory = reinterpret_cast(&free_memory) 40 | - reinterpret_cast(__brkval); 41 | } 42 | return free_memory; 43 | } 44 | //------------------------------------------------------------------------------ 45 | /** %Print a string in flash memory. 46 | * 47 | * \param[in] pr Print object for output. 48 | * \param[in] str Pointer to string stored in flash memory. 49 | */ 50 | void SdFatUtil::print_P( PGM_P str) { 51 | for (uint8_t c; (c = pgm_read_byte(str)); str++) MYSERIAL.write(c); 52 | } 53 | //------------------------------------------------------------------------------ 54 | /** %Print a string in flash memory followed by a CR/LF. 55 | * 56 | * \param[in] pr Print object for output. 57 | * \param[in] str Pointer to string stored in flash memory. 58 | */ 59 | void SdFatUtil::println_P( PGM_P str) { 60 | print_P( str); 61 | MYSERIAL.println(); 62 | } 63 | //------------------------------------------------------------------------------ 64 | /** %Print a string in flash memory to Serial. 65 | * 66 | * \param[in] str Pointer to string stored in flash memory. 67 | */ 68 | void SdFatUtil::SerialPrint_P(PGM_P str) { 69 | print_P(str); 70 | } 71 | //------------------------------------------------------------------------------ 72 | /** %Print a string in flash memory to Serial followed by a CR/LF. 73 | * 74 | * \param[in] str Pointer to string stored in flash memory. 75 | */ 76 | void SdFatUtil::SerialPrintln_P(PGM_P str) { 77 | println_P( str); 78 | } 79 | #endif 80 | -------------------------------------------------------------------------------- /cardreader.h: -------------------------------------------------------------------------------- 1 | #ifndef CARDREADER_H 2 | #define CARDREADER_H 3 | 4 | #ifdef SDSUPPORT 5 | 6 | #define MAX_DIR_DEPTH 10 7 | 8 | #include "SdFile.h" 9 | enum LsAction {LS_SerialPrint,LS_Count,LS_GetFilename}; 10 | class CardReader 11 | { 12 | public: 13 | CardReader(); 14 | 15 | void initsd(); 16 | void write_command(char *buf); 17 | //files auto[0-9].g on the sd card are performed in a row 18 | //this is to delay autostart and hence the initialisaiton of the sd card to some seconds after the normal init, so the device is available quick after a reset 19 | 20 | void checkautostart(bool x); 21 | void openFile(char* name,bool read,bool replace_current=true); 22 | void openLogFile(char* name); 23 | void removeFile(char* name); 24 | void closefile(bool store_location=false); 25 | void release(); 26 | void startFileprint(); 27 | void pauseSDPrint(); 28 | void getStatus(); 29 | void printingHasFinished(); 30 | 31 | void getfilename(const uint8_t nr); 32 | uint16_t getnrfilenames(); 33 | 34 | void getAbsFilename(char *t); 35 | 36 | 37 | void ls(); 38 | void chdir(const char * relpath); 39 | void updir(); 40 | void setroot(); 41 | 42 | 43 | FORCE_INLINE bool isFileOpen() { return file.isOpen(); } 44 | FORCE_INLINE bool eof() { return sdpos>=filesize ;}; 45 | FORCE_INLINE int16_t get() { sdpos = file.curPosition();return (int16_t)file.read();}; 46 | FORCE_INLINE void setIndex(long index) {sdpos = index;file.seekSet(index);}; 47 | FORCE_INLINE uint8_t percentDone(){if(!isFileOpen()) return 0; if(filesize) return sdpos/((filesize+99)/100); else return 0;}; 48 | FORCE_INLINE char* getWorkDirName(){workDir.getFilename(filename);return filename;}; 49 | 50 | public: 51 | bool saving; 52 | bool logging; 53 | bool sdprinting ; 54 | bool cardOK ; 55 | char filename[13]; 56 | char longFilename[LONG_FILENAME_LENGTH]; 57 | bool filenameIsDir; 58 | int lastnr; //last number of the autostart; 59 | private: 60 | SdFile root,*curDir,workDir,workDirParents[MAX_DIR_DEPTH]; 61 | uint16_t workDirDepth; 62 | Sd2Card card; 63 | SdVolume volume; 64 | SdFile file; 65 | #define SD_PROCEDURE_DEPTH 1 66 | #define MAXPATHNAMELENGTH (13*MAX_DIR_DEPTH+MAX_DIR_DEPTH+1) 67 | uint8_t file_subcall_ctr; 68 | uint32_t filespos[SD_PROCEDURE_DEPTH]; 69 | char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH]; 70 | uint32_t filesize; 71 | //int16_t n; 72 | unsigned long autostart_atmillis; 73 | uint32_t sdpos ; 74 | 75 | bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware. 76 | 77 | LsAction lsAction; //stored for recursion. 78 | int16_t nrFiles; //counter for the files in the current directory and recycled as position counter for getting the nrFiles'th name in the directory. 79 | char* diveDirName; 80 | void lsDive(const char *prepend,SdFile parent); 81 | }; 82 | extern CardReader card; 83 | #define IS_SD_PRINTING (card.sdprinting) 84 | 85 | #if (SDCARDDETECT > -1) 86 | # ifdef SDCARDDETECTINVERTED 87 | # define IS_SD_INSERTED (READ(SDCARDDETECT)!=0) 88 | # else 89 | # define IS_SD_INSERTED (READ(SDCARDDETECT)==0) 90 | # endif //SDCARDTETECTINVERTED 91 | #else 92 | //If we don't have a card detect line, aways asume the card is inserted 93 | # define IS_SD_INSERTED true 94 | #endif 95 | 96 | #else 97 | 98 | #define IS_SD_PRINTING (false) 99 | 100 | #endif //SDSUPPORT 101 | #endif 102 | -------------------------------------------------------------------------------- /AjgHardwareSerial.h: -------------------------------------------------------------------------------- 1 | /* 2 | HardwareSerial.h - Hardware serial library for Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 28 September 2010 by Mark Sproul 20 | Modified 14 August 2012 by Alarus 21 | */ 22 | 23 | #ifndef AjgHardwareSerial_h 24 | #define AjgHardwareSerial_h 25 | 26 | #include 27 | 28 | #include "Stream.h" 29 | 30 | struct ring_buffer; 31 | 32 | class AjgHardwareSerial : public Stream 33 | { 34 | private: 35 | ring_buffer *_rx_buffer; 36 | ring_buffer *_tx_buffer; 37 | volatile uint8_t *_ubrrh; 38 | volatile uint8_t *_ubrrl; 39 | volatile uint8_t *_ucsra; 40 | volatile uint8_t *_ucsrb; 41 | volatile uint8_t *_ucsrc; 42 | volatile uint8_t *_udr; 43 | uint8_t _rxen; 44 | uint8_t _txen; 45 | uint8_t _rxcie; 46 | uint8_t _udrie; 47 | uint8_t _u2x; 48 | bool transmitting; 49 | public: 50 | AjgHardwareSerial(ring_buffer *rx_buffer, ring_buffer *tx_buffer, 51 | volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, 52 | volatile uint8_t *ucsra, volatile uint8_t *ucsrb, 53 | volatile uint8_t *ucsrc, volatile uint8_t *udr, 54 | uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udrie, uint8_t u2x); 55 | void begin(unsigned long); 56 | void begin(unsigned long, uint8_t); 57 | void end(); 58 | virtual int available(void); 59 | virtual int peek(void); 60 | virtual int read(void); 61 | virtual void flush(void); 62 | virtual size_t write(uint8_t); 63 | inline size_t write(unsigned long n) { return write((uint8_t)n); } 64 | inline size_t write(long n) { return write((uint8_t)n); } 65 | inline size_t write(unsigned int n) { return write((uint8_t)n); } 66 | inline size_t write(int n) { return write((uint8_t)n); } 67 | using Print::write; // pull in write(str) and write(buf, size) from Print 68 | operator bool(); 69 | }; 70 | 71 | // Define config for Serial.begin(baud, config); 72 | #define SERIAL_5N1 0x00 73 | #define SERIAL_6N1 0x02 74 | #define SERIAL_7N1 0x04 75 | #define SERIAL_8N1 0x06 76 | #define SERIAL_5N2 0x08 77 | #define SERIAL_6N2 0x0A 78 | #define SERIAL_7N2 0x0C 79 | #define SERIAL_8N2 0x0E 80 | #define SERIAL_5E1 0x20 81 | #define SERIAL_6E1 0x22 82 | #define SERIAL_7E1 0x24 83 | #define SERIAL_8E1 0x26 84 | #define SERIAL_5E2 0x28 85 | #define SERIAL_6E2 0x2A 86 | #define SERIAL_7E2 0x2C 87 | #define SERIAL_8E2 0x2E 88 | #define SERIAL_5O1 0x30 89 | #define SERIAL_6O1 0x32 90 | #define SERIAL_7O1 0x34 91 | #define SERIAL_8O1 0x36 92 | #define SERIAL_5O2 0x38 93 | #define SERIAL_6O2 0x3A 94 | #define SERIAL_7O2 0x3C 95 | #define SERIAL_8O2 0x3E 96 | 97 | #if defined(UBRR3H) 98 | extern AjgHardwareSerial AjgSerial; 99 | #endif 100 | 101 | extern void serialEventRun(void) __attribute__((weak)); 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /SdFile.cpp: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | 22 | #ifdef SDSUPPORT 23 | #include "SdFile.h" 24 | /** Create a file object and open it in the current working directory. 25 | * 26 | * \param[in] path A path with a valid 8.3 DOS name for a file to be opened. 27 | * 28 | * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive 29 | * OR of open flags. see SdBaseFile::open(SdBaseFile*, const char*, uint8_t). 30 | */ 31 | SdFile::SdFile(const char* path, uint8_t oflag) : SdBaseFile(path, oflag) { 32 | } 33 | //------------------------------------------------------------------------------ 34 | /** Write data to an open file. 35 | * 36 | * \note Data is moved to the cache but may not be written to the 37 | * storage device until sync() is called. 38 | * 39 | * \param[in] buf Pointer to the location of the data to be written. 40 | * 41 | * \param[in] nbyte Number of bytes to write. 42 | * 43 | * \return For success write() returns the number of bytes written, always 44 | * \a nbyte. If an error occurs, write() returns -1. Possible errors 45 | * include write() is called before a file has been opened, write is called 46 | * for a read-only file, device is full, a corrupt file system or an I/O error. 47 | * 48 | */ 49 | int16_t SdFile::write(const void* buf, uint16_t nbyte) { 50 | return SdBaseFile::write(buf, nbyte); 51 | } 52 | //------------------------------------------------------------------------------ 53 | /** Write a byte to a file. Required by the Arduino Print class. 54 | * \param[in] b the byte to be written. 55 | * Use writeError to check for errors. 56 | */ 57 | #if ARDUINO >= 100 58 | size_t SdFile::write(uint8_t b) 59 | { 60 | return SdBaseFile::write(&b, 1); 61 | } 62 | #else 63 | void SdFile::write(uint8_t b) 64 | { 65 | SdBaseFile::write(&b, 1); 66 | } 67 | #endif 68 | //------------------------------------------------------------------------------ 69 | /** Write a string to a file. Used by the Arduino Print class. 70 | * \param[in] str Pointer to the string. 71 | * Use writeError to check for errors. 72 | */ 73 | void SdFile::write(const char* str) { 74 | SdBaseFile::write(str, strlen(str)); 75 | } 76 | //------------------------------------------------------------------------------ 77 | /** Write a PROGMEM string to a file. 78 | * \param[in] str Pointer to the PROGMEM string. 79 | * Use writeError to check for errors. 80 | */ 81 | void SdFile::write_P(PGM_P str) { 82 | for (uint8_t c; (c = pgm_read_byte(str)); str++) write(c); 83 | } 84 | //------------------------------------------------------------------------------ 85 | /** Write a PROGMEM string followed by CR/LF to a file. 86 | * \param[in] str Pointer to the PROGMEM string. 87 | * Use writeError to check for errors. 88 | */ 89 | void SdFile::writeln_P(PGM_P str) { 90 | write_P(str); 91 | write_P(PSTR("\r\n")); 92 | } 93 | 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /LiquidCrystalRus.h: -------------------------------------------------------------------------------- 1 | // 2 | // based on LiquidCrystal library from ArduinoIDE, see http://arduino.cc 3 | // modified 27 Jul 2011 4 | // by Ilya V. Danilov http://mk90.ru/ 5 | // 6 | 7 | #ifndef LiquidCrystalRus_h 8 | #define LiquidCrystalRus_h 9 | 10 | #include 11 | #include "Print.h" 12 | 13 | // commands 14 | #define LCD_CLEARDISPLAY 0x01 15 | #define LCD_RETURNHOME 0x02 16 | #define LCD_ENTRYMODESET 0x04 17 | #define LCD_DISPLAYCONTROL 0x08 18 | #define LCD_CURSORSHIFT 0x10 19 | #define LCD_FUNCTIONSET 0x20 20 | #define LCD_SETCGRAMADDR 0x40 21 | #define LCD_SETDDRAMADDR 0x80 22 | 23 | // flags for display entry mode 24 | #define LCD_ENTRYRIGHT 0x00 25 | #define LCD_ENTRYLEFT 0x02 26 | #define LCD_ENTRYSHIFTINCREMENT 0x01 27 | #define LCD_ENTRYSHIFTDECREMENT 0x00 28 | 29 | // flags for display on/off control 30 | #define LCD_DISPLAYON 0x04 31 | #define LCD_DISPLAYOFF 0x00 32 | #define LCD_CURSORON 0x02 33 | #define LCD_CURSOROFF 0x00 34 | #define LCD_BLINKON 0x01 35 | #define LCD_BLINKOFF 0x00 36 | 37 | // flags for display/cursor shift 38 | #define LCD_DISPLAYMOVE 0x08 39 | #define LCD_CURSORMOVE 0x00 40 | #define LCD_MOVERIGHT 0x04 41 | #define LCD_MOVELEFT 0x00 42 | 43 | // flags for function set 44 | #define LCD_8BITMODE 0x10 45 | #define LCD_4BITMODE 0x00 46 | #define LCD_2LINE 0x08 47 | #define LCD_1LINE 0x00 48 | #define LCD_5x10DOTS 0x04 49 | #define LCD_5x8DOTS 0x00 50 | 51 | // enum for 52 | #define LCD_DRAM_Normal 0x00 53 | #define LCD_DRAM_WH1601 0x01 54 | 55 | 56 | class LiquidCrystalRus : public Print { 57 | public: 58 | LiquidCrystalRus(uint8_t rs, uint8_t enable, 59 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 60 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); 61 | LiquidCrystalRus(uint8_t rs, uint8_t rw, uint8_t enable, 62 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 63 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); 64 | LiquidCrystalRus(uint8_t rs, uint8_t rw, uint8_t enable, 65 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3); 66 | LiquidCrystalRus(uint8_t rs, uint8_t enable, 67 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3); 68 | 69 | void init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable, 70 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 71 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); 72 | 73 | void begin(uint8_t cols, uint8_t rows, uint8_t charsize = LCD_5x8DOTS); 74 | 75 | void clear(); 76 | void home(); 77 | 78 | void noDisplay(); 79 | void display(); 80 | void noBlink(); 81 | void blink(); 82 | void noCursor(); 83 | void cursor(); 84 | void scrollDisplayLeft(); 85 | void scrollDisplayRight(); 86 | void leftToRight(); 87 | void rightToLeft(); 88 | void autoscroll(); 89 | void noAutoscroll(); 90 | 91 | void createChar(uint8_t, uint8_t[]); 92 | void setCursor(uint8_t, uint8_t); 93 | 94 | #if defined(ARDUINO) && ARDUINO >= 100 95 | virtual size_t write(uint8_t); 96 | using Print::write; 97 | #else 98 | virtual void write(uint8_t); 99 | #endif 100 | 101 | void command(uint8_t); 102 | 103 | void setDRAMModel(uint8_t); 104 | 105 | private: 106 | void send(uint8_t, uint8_t); 107 | void writeNbits(uint8_t, uint8_t); 108 | uint8_t recv(uint8_t); 109 | uint8_t readNbits(uint8_t); 110 | void pulseEnable(); 111 | 112 | uint8_t _rs_pin; // LOW: command. HIGH: character. 113 | uint8_t _rw_pin; // LOW: write to LCD. HIGH: read from LCD. 114 | uint8_t _enable_pin; // activated by a HIGH pulse. 115 | uint8_t _data_pins[8]; 116 | 117 | uint8_t _displayfunction; 118 | uint8_t _displaycontrol; 119 | uint8_t _displaymode; 120 | 121 | uint8_t _initialized; 122 | 123 | uint8_t _numlines,_currline; 124 | 125 | uint8_t _dram_model; 126 | uint8_t utf_hi_char; // UTF-8 high part 127 | }; 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /ultralcd.h: -------------------------------------------------------------------------------- 1 | #ifndef ULTRALCD_H 2 | #define ULTRALCD_H 3 | 4 | #include "Marlin.h" 5 | 6 | #ifdef ULTRA_LCD 7 | 8 | void lcd_update(); 9 | void lcd_init(); 10 | void lcd_setstatus(const char* message); 11 | void lcd_setstatuspgm(const char* message); 12 | void lcd_setalertstatuspgm(const char* message); 13 | void lcd_reset_alert_level(); 14 | 15 | #ifdef DOGLCD 16 | extern int lcd_contrast; 17 | void lcd_setcontrast(uint8_t value); 18 | #endif 19 | 20 | static unsigned char blink = 0; // Variable for visualization of fan rotation in GLCD 21 | 22 | #define LCD_MESSAGEPGM(x) lcd_setstatuspgm(PSTR(x)) 23 | #define LCD_ALERTMESSAGEPGM(x) lcd_setalertstatuspgm(PSTR(x)) 24 | 25 | #define LCD_UPDATE_INTERVAL 100 26 | #define LCD_TIMEOUT_TO_STATUS 15000 27 | 28 | #ifdef ULTIPANEL 29 | void lcd_buttons_update(); 30 | extern volatile uint8_t buttons; //the last checked buttons in a bit array. 31 | #ifdef REPRAPWORLD_KEYPAD 32 | extern volatile uint8_t buttons_reprapworld_keypad; // to store the keypad shift register values 33 | #endif 34 | #else 35 | FORCE_INLINE void lcd_buttons_update() {} 36 | #endif 37 | 38 | extern int plaPreheatHotendTemp; 39 | extern int plaPreheatHPBTemp; 40 | extern int plaPreheatFanSpeed; 41 | 42 | extern int absPreheatHotendTemp; 43 | extern int absPreheatHPBTemp; 44 | extern int absPreheatFanSpeed; 45 | 46 | extern bool cancel_heatup; 47 | 48 | void lcd_buzz(long duration,uint16_t freq); 49 | bool lcd_clicked(); 50 | 51 | #ifdef NEWPANEL 52 | #define EN_C (1< 23 | 24 | static void ST7920_SWSPI_SND_8BIT(uint8_t val) 25 | { 26 | uint8_t i; 27 | for( i=0; i<8; i++ ) 28 | { 29 | WRITE(ST7920_CLK_PIN,0); 30 | WRITE(ST7920_DAT_PIN,val&0x80); 31 | val<<=1; 32 | WRITE(ST7920_CLK_PIN,1); 33 | } 34 | } 35 | 36 | #define ST7920_CS() {WRITE(ST7920_CS_PIN,1);u8g_10MicroDelay();} 37 | #define ST7920_NCS() {WRITE(ST7920_CS_PIN,0);} 38 | #define ST7920_SET_CMD() {ST7920_SWSPI_SND_8BIT(0xf8);u8g_10MicroDelay();} 39 | #define ST7920_SET_DAT() {ST7920_SWSPI_SND_8BIT(0xfa);u8g_10MicroDelay();} 40 | #define ST7920_WRITE_BYTE(a) {ST7920_SWSPI_SND_8BIT((a)&0xf0);ST7920_SWSPI_SND_8BIT((a)<<4);u8g_10MicroDelay();} 41 | #define ST7920_WRITE_BYTES(p,l) {uint8_t i;for(i=0;idev_mem); 84 | y = pb->p.page_y0; 85 | ptr = (uint8_t*)pb->buf; 86 | 87 | ST7920_CS(); 88 | for( i = 0; i < PAGE_HEIGHT; i ++ ) 89 | { 90 | ST7920_SET_CMD(); 91 | if ( y < 32 ) 92 | { 93 | ST7920_WRITE_BYTE(0x80 | y); //y 94 | ST7920_WRITE_BYTE(0x80); //x=0 95 | } 96 | else 97 | { 98 | ST7920_WRITE_BYTE(0x80 | (y-32)); //y 99 | ST7920_WRITE_BYTE(0x80 | 8); //x=64 100 | } 101 | 102 | ST7920_SET_DAT(); 103 | ST7920_WRITE_BYTES(ptr,WIDTH/8); //ptr is incremented inside of macro 104 | y++; 105 | } 106 | ST7920_NCS(); 107 | } 108 | break; 109 | } 110 | #if PAGE_HEIGHT == 8 111 | return u8g_dev_pb8h1_base_fn(u8g, dev, msg, arg); 112 | #elif PAGE_HEIGHT == 16 113 | return u8g_dev_pb16h1_base_fn(u8g, dev, msg, arg); 114 | #else 115 | return u8g_dev_pb32h1_base_fn(u8g, dev, msg, arg); 116 | #endif 117 | } 118 | 119 | uint8_t u8g_dev_st7920_128x64_rrd_buf[WIDTH*(PAGE_HEIGHT/8)] U8G_NOCOMMON; 120 | u8g_pb_t u8g_dev_st7920_128x64_rrd_pb = {{PAGE_HEIGHT,HEIGHT,0,0,0},WIDTH,u8g_dev_st7920_128x64_rrd_buf}; 121 | u8g_dev_t u8g_dev_st7920_128x64_rrd_sw_spi = {u8g_dev_rrd_st7920_128x64_fn,&u8g_dev_st7920_128x64_rrd_pb,&u8g_com_null_fn}; 122 | 123 | class U8GLIB_ST7920_128X64_RRD : public U8GLIB 124 | { 125 | public: 126 | U8GLIB_ST7920_128X64_RRD(uint8_t dummy) : U8GLIB(&u8g_dev_st7920_128x64_rrd_sw_spi) {} 127 | }; 128 | 129 | 130 | #endif //U8GLIB_ST7920 131 | #endif //ULCDST7920_H 132 | -------------------------------------------------------------------------------- /SdFatConfig.h: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | /** 21 | * \file 22 | * \brief configuration definitions 23 | */ 24 | #include "Marlin.h" 25 | #ifdef SDSUPPORT 26 | 27 | #ifndef SdFatConfig_h 28 | #define SdFatConfig_h 29 | #include 30 | //------------------------------------------------------------------------------ 31 | /** 32 | * To use multiple SD cards set USE_MULTIPLE_CARDS nonzero. 33 | * 34 | * Using multiple cards costs 400 - 500 bytes of flash. 35 | * 36 | * Each card requires about 550 bytes of SRAM so use of a Mega is recommended. 37 | */ 38 | #define USE_MULTIPLE_CARDS 0 39 | //------------------------------------------------------------------------------ 40 | /** 41 | * Call flush for endl if ENDL_CALLS_FLUSH is nonzero 42 | * 43 | * The standard for iostreams is to call flush. This is very costly for 44 | * SdFat. Each call to flush causes 2048 bytes of I/O to the SD. 45 | * 46 | * SdFat has a single 512 byte buffer for SD I/O so it must write the current 47 | * data block to the SD, read the directory block from the SD, update the 48 | * directory entry, write the directory block to the SD and read the data 49 | * block back into the buffer. 50 | * 51 | * The SD flash memory controller is not designed for this many rewrites 52 | * so performance may be reduced by more than a factor of 100. 53 | * 54 | * If ENDL_CALLS_FLUSH is zero, you must call flush and/or close to force 55 | * all data to be written to the SD. 56 | */ 57 | #define ENDL_CALLS_FLUSH 0 58 | //------------------------------------------------------------------------------ 59 | /** 60 | * Allow use of deprecated functions if ALLOW_DEPRECATED_FUNCTIONS is nonzero 61 | */ 62 | #define ALLOW_DEPRECATED_FUNCTIONS 1 63 | //------------------------------------------------------------------------------ 64 | /** 65 | * Allow FAT12 volumes if FAT12_SUPPORT is nonzero. 66 | * FAT12 has not been well tested. 67 | */ 68 | #define FAT12_SUPPORT 0 69 | //------------------------------------------------------------------------------ 70 | /** 71 | * SPI init rate for SD initialization commands. Must be 5 (F_CPU/64) 72 | * or 6 (F_CPU/128). 73 | */ 74 | #define SPI_SD_INIT_RATE 5 75 | //------------------------------------------------------------------------------ 76 | /** 77 | * Set the SS pin high for hardware SPI. If SS is chip select for another SPI 78 | * device this will disable that device during the SD init phase. 79 | */ 80 | #define SET_SPI_SS_HIGH 1 81 | //------------------------------------------------------------------------------ 82 | /** 83 | * Define MEGA_SOFT_SPI nonzero to use software SPI on Mega Arduinos. 84 | * Pins used are SS 10, MOSI 11, MISO 12, and SCK 13. 85 | * 86 | * MEGA_SOFT_SPI allows an unmodified Adafruit GPS Shield to be used 87 | * on Mega Arduinos. Software SPI works well with GPS Shield V1.1 88 | * but many SD cards will fail with GPS Shield V1.0. 89 | */ 90 | #define MEGA_SOFT_SPI 0 91 | //------------------------------------------------------------------------------ 92 | /** 93 | * Set USE_SOFTWARE_SPI nonzero to always use software SPI. 94 | */ 95 | #define USE_SOFTWARE_SPI 0 96 | // define software SPI pins so Mega can use unmodified 168/328 shields 97 | /** Software SPI chip select pin for the SD */ 98 | uint8_t const SOFT_SPI_CS_PIN = 10; 99 | /** Software SPI Master Out Slave In pin */ 100 | uint8_t const SOFT_SPI_MOSI_PIN = 11; 101 | /** Software SPI Master In Slave Out pin */ 102 | uint8_t const SOFT_SPI_MISO_PIN = 12; 103 | /** Software SPI Clock pin */ 104 | uint8_t const SOFT_SPI_SCK_PIN = 13; 105 | //------------------------------------------------------------------------------ 106 | /** 107 | * The __cxa_pure_virtual function is an error handler that is invoked when 108 | * a pure virtual function is called. 109 | */ 110 | #define USE_CXA_PURE_VIRTUAL 1 111 | /** 112 | * Defines for long (vfat) filenames 113 | */ 114 | /** Number of VFAT entries used. Every entry has 13 UTF-16 characters */ 115 | #define MAX_VFAT_ENTRIES (2) 116 | /** Total size of the buffer used to store the long filenames */ 117 | #define LONG_FILENAME_LENGTH (13*MAX_VFAT_ENTRIES+1) 118 | #endif // SdFatConfig_h 119 | 120 | 121 | #endif 122 | -------------------------------------------------------------------------------- /stepper.h: -------------------------------------------------------------------------------- 1 | /* 2 | stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors 3 | Part of Grbl 4 | 5 | Copyright (c) 2009-2011 Simen Svale Skogsrud 6 | 7 | Grbl is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | Grbl is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with Grbl. If not, see . 19 | */ 20 | 21 | #ifndef stepper_h 22 | #define stepper_h 23 | 24 | #include "planner.h" 25 | 26 | #if EXTRUDERS > 2 27 | #define WRITE_E_STEP(v) { if(current_block->active_extruder == 2) { WRITE(E2_STEP_PIN, v); } else { if(current_block->active_extruder == 1) { WRITE(E1_STEP_PIN, v); } else { WRITE(E0_STEP_PIN, v); }}} 28 | #define NORM_E_DIR() { if(current_block->active_extruder == 2) { WRITE(E2_DIR_PIN, !INVERT_E2_DIR); } else { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); }}} 29 | #define REV_E_DIR() { if(current_block->active_extruder == 2) { WRITE(E2_DIR_PIN, INVERT_E2_DIR); } else { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, INVERT_E0_DIR); }}} 30 | #elif EXTRUDERS > 1 31 | #ifndef DUAL_X_CARRIAGE 32 | #define WRITE_E_STEP(v) { if(current_block->active_extruder == 1) { WRITE(E1_STEP_PIN, v); } else { WRITE(E0_STEP_PIN, v); }} 33 | #define NORM_E_DIR() { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); }} 34 | #define REV_E_DIR() { if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, INVERT_E0_DIR); }} 35 | #else 36 | extern bool extruder_duplication_enabled; 37 | #define WRITE_E_STEP(v) { if(extruder_duplication_enabled) { WRITE(E0_STEP_PIN, v); WRITE(E1_STEP_PIN, v); } else if(current_block->active_extruder == 1) { WRITE(E1_STEP_PIN, v); } else { WRITE(E0_STEP_PIN, v); }} 38 | #define NORM_E_DIR() { if(extruder_duplication_enabled) { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, !INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, !INVERT_E0_DIR); }} 39 | #define REV_E_DIR() { if(extruder_duplication_enabled) { WRITE(E0_DIR_PIN, INVERT_E0_DIR); WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else if(current_block->active_extruder == 1) { WRITE(E1_DIR_PIN, INVERT_E1_DIR); } else { WRITE(E0_DIR_PIN, INVERT_E0_DIR); }} 40 | #endif 41 | #else 42 | #define WRITE_E_STEP(v) WRITE(E0_STEP_PIN, v) 43 | #define NORM_E_DIR() WRITE(E0_DIR_PIN, !INVERT_E0_DIR) 44 | #define REV_E_DIR() WRITE(E0_DIR_PIN, INVERT_E0_DIR) 45 | #endif 46 | 47 | #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED 48 | extern bool abort_on_endstop_hit; 49 | #endif 50 | 51 | // Initialize and start the stepper motor subsystem 52 | void st_init(); 53 | 54 | // Block until all buffered steps are executed 55 | void st_synchronize(); 56 | 57 | // Set current position in steps 58 | void st_set_position(const long &x, const long &y, const long &z, const long &e); 59 | void st_set_e_position(const long &e); 60 | 61 | // Get current position in steps 62 | long st_get_position(uint8_t axis); 63 | 64 | #ifdef ENABLE_AUTO_BED_LEVELING 65 | // Get current position in mm 66 | float st_get_position_mm(uint8_t axis); 67 | #endif //ENABLE_AUTO_BED_LEVELING 68 | 69 | // The stepper subsystem goes to sleep when it runs out of things to execute. Call this 70 | // to notify the subsystem that it is time to go to work. 71 | void st_wake_up(); 72 | 73 | 74 | void checkHitEndstops(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered 75 | void endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homing and before a routine call of checkHitEndstops(); 76 | 77 | void enable_endstops(bool check); // Enable/disable endstop checking 78 | 79 | void checkStepperErrors(); //Print errors detected by the stepper 80 | 81 | void finishAndDisableSteppers(); 82 | 83 | extern block_t *current_block; // A pointer to the block currently being traced 84 | 85 | void quickStop(); 86 | 87 | void digitalPotWrite(int address, int value); 88 | void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2); 89 | void microstep_mode(uint8_t driver, uint8_t stepping); 90 | void digipot_init(); 91 | void digipot_current(uint8_t driver, int current); 92 | void microstep_init(); 93 | void microstep_readings(); 94 | 95 | #ifdef BABYSTEPPING 96 | void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention 97 | #endif 98 | 99 | 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /vector_3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | vector_3.cpp - Vector library for bed leveling 3 | Copyright (c) 2012 Lars Brubaker. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | #include 20 | #include "Marlin.h" 21 | 22 | #ifdef ENABLE_AUTO_BED_LEVELING 23 | #include "vector_3.h" 24 | 25 | vector_3::vector_3() : x(0), y(0), z(0) { } 26 | 27 | vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { } 28 | 29 | vector_3 vector_3::cross(vector_3 left, vector_3 right) 30 | { 31 | return vector_3(left.y * right.z - left.z * right.y, 32 | left.z * right.x - left.x * right.z, 33 | left.x * right.y - left.y * right.x); 34 | } 35 | 36 | vector_3 vector_3::operator+(vector_3 v) 37 | { 38 | return vector_3((x + v.x), (y + v.y), (z + v.z)); 39 | } 40 | 41 | vector_3 vector_3::operator-(vector_3 v) 42 | { 43 | return vector_3((x - v.x), (y - v.y), (z - v.z)); 44 | } 45 | 46 | vector_3 vector_3::get_normal() 47 | { 48 | vector_3 normalized = vector_3(x, y, z); 49 | normalized.normalize(); 50 | return normalized; 51 | } 52 | 53 | float vector_3::get_length() 54 | { 55 | float length = sqrt((x * x) + (y * y) + (z * z)); 56 | return length; 57 | } 58 | 59 | void vector_3::normalize() 60 | { 61 | float length = get_length(); 62 | x /= length; 63 | y /= length; 64 | z /= length; 65 | } 66 | 67 | void vector_3::apply_rotation(matrix_3x3 matrix) 68 | { 69 | float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0]; 70 | float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1]; 71 | float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2]; 72 | 73 | x = resultX; 74 | y = resultY; 75 | z = resultZ; 76 | } 77 | 78 | void vector_3::debug(char* title) 79 | { 80 | SERIAL_PROTOCOL(title); 81 | SERIAL_PROTOCOLPGM(" x: "); 82 | SERIAL_PROTOCOL(x); 83 | SERIAL_PROTOCOLPGM(" y: "); 84 | SERIAL_PROTOCOL(y); 85 | SERIAL_PROTOCOLPGM(" z: "); 86 | SERIAL_PROTOCOL(z); 87 | SERIAL_PROTOCOLPGM("\n"); 88 | } 89 | 90 | void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z) 91 | { 92 | vector_3 vector = vector_3(x, y, z); 93 | vector.apply_rotation(matrix); 94 | x = vector.x; 95 | y = vector.y; 96 | z = vector.z; 97 | } 98 | 99 | matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) 100 | { 101 | //row_0.debug("row_0"); 102 | //row_1.debug("row_1"); 103 | //row_2.debug("row_2"); 104 | matrix_3x3 new_matrix; 105 | new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z; 106 | new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z; 107 | new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z; 108 | //new_matrix.debug("new_matrix"); 109 | 110 | return new_matrix; 111 | } 112 | 113 | void matrix_3x3::set_to_identity() 114 | { 115 | matrix[0] = 1; matrix[1] = 0; matrix[2] = 0; 116 | matrix[3] = 0; matrix[4] = 1; matrix[5] = 0; 117 | matrix[6] = 0; matrix[7] = 0; matrix[8] = 1; 118 | } 119 | 120 | matrix_3x3 matrix_3x3::create_look_at(vector_3 target) 121 | { 122 | vector_3 z_row = target.get_normal(); 123 | vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal(); 124 | vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); 125 | 126 | // x_row.debug("x_row"); 127 | // y_row.debug("y_row"); 128 | // z_row.debug("z_row"); 129 | 130 | 131 | // create the matrix already correctly transposed 132 | matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); 133 | 134 | // rot.debug("rot"); 135 | return rot; 136 | } 137 | 138 | 139 | matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) 140 | { 141 | matrix_3x3 new_matrix; 142 | new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6]; 143 | new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7]; 144 | new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8]; 145 | return new_matrix; 146 | } 147 | 148 | void matrix_3x3::debug(char* title) 149 | { 150 | SERIAL_PROTOCOL(title); 151 | SERIAL_PROTOCOL("\n"); 152 | int count = 0; 153 | for(int i=0; i<3; i++) 154 | { 155 | for(int j=0; j<3; j++) 156 | { 157 | SERIAL_PROTOCOL(matrix[count]); 158 | SERIAL_PROTOCOLPGM(" "); 159 | count++; 160 | } 161 | 162 | SERIAL_PROTOCOLPGM("\n"); 163 | } 164 | } 165 | 166 | #endif // #ifdef ENABLE_AUTO_BED_LEVELING 167 | 168 | -------------------------------------------------------------------------------- /temperature.h: -------------------------------------------------------------------------------- 1 | /* 2 | temperature.h - temperature controller 3 | Part of Marlin 4 | 5 | Copyright (c) 2011 Erik van der Zalm 6 | 7 | Grbl is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | Grbl is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with Grbl. If not, see . 19 | */ 20 | 21 | #ifndef temperature_h 22 | #define temperature_h 23 | 24 | #include "Marlin.h" 25 | #include "planner.h" 26 | #ifdef PID_ADD_EXTRUSION_RATE 27 | #include "stepper.h" 28 | #endif 29 | 30 | // public functions 31 | void tp_init(); //initialize the heating 32 | void manage_heater(); //it is critical that this is called periodically. 33 | 34 | #ifdef FILAMENT_SENSOR 35 | // For converting raw Filament Width to milimeters 36 | float analog2widthFil(); 37 | 38 | // For converting raw Filament Width to an extrusion ratio 39 | int widthFil_to_size_ratio(); 40 | #endif 41 | 42 | // low level conversion routines 43 | // do not use these routines and variables outside of temperature.cpp 44 | extern int target_temperature[EXTRUDERS]; 45 | extern float current_temperature[EXTRUDERS]; 46 | #ifdef SHOW_TEMP_ADC_VALUES 47 | extern int current_temperature_raw[EXTRUDERS]; 48 | extern int current_temperature_bed_raw; 49 | #endif 50 | extern int target_temperature_bed; 51 | extern float current_temperature_bed; 52 | #ifdef TEMP_SENSOR_1_AS_REDUNDANT 53 | extern float redundant_temperature; 54 | #endif 55 | 56 | #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1 57 | extern unsigned char soft_pwm_bed; 58 | #endif 59 | 60 | #ifdef PIDTEMP 61 | extern float Kp,Ki,Kd,Kc; 62 | float scalePID_i(float i); 63 | float scalePID_d(float d); 64 | float unscalePID_i(float i); 65 | float unscalePID_d(float d); 66 | 67 | #endif 68 | #ifdef PIDTEMPBED 69 | extern float bedKp,bedKi,bedKd; 70 | #endif 71 | 72 | 73 | #ifdef BABYSTEPPING 74 | extern volatile int babystepsTodo[3]; 75 | #endif 76 | 77 | //high level conversion routines, for use outside of temperature.cpp 78 | //inline so that there is no performance decrease. 79 | //deg=degreeCelsius 80 | 81 | FORCE_INLINE float degHotend(uint8_t extruder) { 82 | return current_temperature[extruder]; 83 | }; 84 | 85 | #ifdef SHOW_TEMP_ADC_VALUES 86 | FORCE_INLINE float rawHotendTemp(uint8_t extruder) { 87 | return current_temperature_raw[extruder]; 88 | }; 89 | 90 | FORCE_INLINE float rawBedTemp() { 91 | return current_temperature_bed_raw; 92 | }; 93 | #endif 94 | 95 | FORCE_INLINE float degBed() { 96 | return current_temperature_bed; 97 | }; 98 | 99 | FORCE_INLINE float degTargetHotend(uint8_t extruder) { 100 | return target_temperature[extruder]; 101 | }; 102 | 103 | FORCE_INLINE float degTargetBed() { 104 | return target_temperature_bed; 105 | }; 106 | 107 | FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) { 108 | target_temperature[extruder] = celsius; 109 | }; 110 | 111 | FORCE_INLINE void setTargetBed(const float &celsius) { 112 | target_temperature_bed = celsius; 113 | }; 114 | 115 | FORCE_INLINE bool isHeatingHotend(uint8_t extruder){ 116 | return target_temperature[extruder] > current_temperature[extruder]; 117 | }; 118 | 119 | FORCE_INLINE bool isHeatingBed() { 120 | return target_temperature_bed > current_temperature_bed; 121 | }; 122 | 123 | FORCE_INLINE bool isCoolingHotend(uint8_t extruder) { 124 | return target_temperature[extruder] < current_temperature[extruder]; 125 | }; 126 | 127 | FORCE_INLINE bool isCoolingBed() { 128 | return target_temperature_bed < current_temperature_bed; 129 | }; 130 | 131 | #define degHotend0() degHotend(0) 132 | #define degTargetHotend0() degTargetHotend(0) 133 | #define setTargetHotend0(_celsius) setTargetHotend((_celsius), 0) 134 | #define isHeatingHotend0() isHeatingHotend(0) 135 | #define isCoolingHotend0() isCoolingHotend(0) 136 | #if EXTRUDERS > 1 137 | #define degHotend1() degHotend(1) 138 | #define degTargetHotend1() degTargetHotend(1) 139 | #define setTargetHotend1(_celsius) setTargetHotend((_celsius), 1) 140 | #define isHeatingHotend1() isHeatingHotend(1) 141 | #define isCoolingHotend1() isCoolingHotend(1) 142 | #else 143 | #define setTargetHotend1(_celsius) do{}while(0) 144 | #endif 145 | #if EXTRUDERS > 2 146 | #define degHotend2() degHotend(2) 147 | #define degTargetHotend2() degTargetHotend(2) 148 | #define setTargetHotend2(_celsius) setTargetHotend((_celsius), 2) 149 | #define isHeatingHotend2() isHeatingHotend(2) 150 | #define isCoolingHotend2() isCoolingHotend(2) 151 | #else 152 | #define setTargetHotend2(_celsius) do{}while(0) 153 | #endif 154 | #if EXTRUDERS > 3 155 | #error Invalid number of extruders 156 | #endif 157 | 158 | 159 | 160 | int getHeaterPower(int heater); 161 | void disable_heater(); 162 | void setWatch(); 163 | void updatePID(); 164 | 165 | #ifdef THERMAL_RUNAWAY_PROTECTION_PERIOD && THERMAL_RUNAWAY_PROTECTION_PERIOD > 0 166 | void thermal_runaway_protection(int *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc); 167 | static int thermal_runaway_state_machine[3]; // = {0,0,0}; 168 | static unsigned long thermal_runaway_timer[3]; // = {0,0,0}; 169 | static bool thermal_runaway = false; 170 | #if TEMP_SENSOR_BED != 0 171 | static int thermal_runaway_bed_state_machine; 172 | static unsigned long thermal_runaway_bed_timer; 173 | #endif 174 | #endif 175 | 176 | FORCE_INLINE void autotempShutdown(){ 177 | #ifdef AUTOTEMP 178 | if(autotemp_enabled) 179 | { 180 | autotemp_enabled=false; 181 | if(degTargetHotend(active_extruder)>autotemp_min) 182 | setTargetHotend(0,active_extruder); 183 | } 184 | #endif 185 | } 186 | 187 | void PID_autotune(float temp, int extruder, int ncycles); 188 | 189 | #endif 190 | 191 | -------------------------------------------------------------------------------- /Servo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | 22 | A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. 23 | The servos are pulsed in the background using the value most recently written using the write() method 24 | 25 | Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. 26 | Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. 27 | The sequence used to seize timers is defined in timers.h 28 | 29 | The methods are: 30 | 31 | Servo - Class for manipulating servo motors connected to Arduino pins. 32 | 33 | attach(pin ) - Attaches a servo motor to an i/o pin. 34 | attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds 35 | default min is 544, max is 2400 36 | 37 | write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) 38 | writeMicroseconds() - Sets the servo pulse width in microseconds 39 | read() - Gets the last written servo pulse width as an angle between 0 and 180. 40 | readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) 41 | attached() - Returns true if there is a servo attached. 42 | detach() - Stops an attached servos from pulsing its i/o pin. 43 | */ 44 | 45 | #ifndef Servo_h 46 | #define Servo_h 47 | 48 | #include 49 | 50 | /* 51 | * Defines for 16 bit timers used with Servo library 52 | * 53 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 54 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 55 | * _Nbr_16timers indicates how many 16 bit timers are available. 56 | * 57 | */ 58 | 59 | // Say which 16 bit timers can be used and in what order 60 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 61 | #define _useTimer5 62 | //#define _useTimer1 63 | #define _useTimer3 64 | #define _useTimer4 65 | //typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; 66 | typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; 67 | 68 | #elif defined(__AVR_ATmega32U4__) 69 | //#define _useTimer1 70 | #define _useTimer3 71 | //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; 72 | typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; 73 | 74 | #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) 75 | #define _useTimer3 76 | //#define _useTimer1 77 | //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; 78 | typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; 79 | 80 | #elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) ||defined(__AVR_ATmega2561__) 81 | #define _useTimer3 82 | //#define _useTimer1 83 | //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; 84 | typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; 85 | 86 | #else // everything else 87 | //#define _useTimer1 88 | //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; 89 | typedef enum { _Nbr_16timers } timer16_Sequence_t ; 90 | #endif 91 | 92 | #define Servo_VERSION 2 // software version of this library 93 | 94 | #define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo 95 | #define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo 96 | #define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached 97 | #define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds 98 | 99 | #define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer 100 | #define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) 101 | 102 | #define INVALID_SERVO 255 // flag indicating an invalid servo index 103 | 104 | typedef struct { 105 | uint8_t nbr :6 ; // a pin number from 0 to 63 106 | uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false 107 | } ServoPin_t ; 108 | 109 | typedef struct { 110 | ServoPin_t Pin; 111 | unsigned int ticks; 112 | } servo_t; 113 | 114 | class Servo 115 | { 116 | public: 117 | Servo(); 118 | uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure 119 | uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 120 | void detach(); 121 | void write(int value); // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds 122 | void writeMicroseconds(int value); // Write pulse width in microseconds 123 | int read(); // returns current pulse width as an angle between 0 and 180 degrees 124 | int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) 125 | bool attached(); // return true if this servo is attached, otherwise false 126 | #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) 127 | int pin; // store the hardware pin of the servo 128 | #endif 129 | private: 130 | uint8_t servoIndex; // index into the channel data for this servo 131 | int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH 132 | int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH 133 | }; 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /MarlinSerial.h: -------------------------------------------------------------------------------- 1 | /* 2 | HardwareSerial.h - Hardware serial library for Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 28 September 2010 by Mark Sproul 20 | */ 21 | 22 | #ifndef MarlinSerial_h 23 | #define MarlinSerial_h 24 | #include "Marlin.h" 25 | 26 | #if !defined(SERIAL_PORT) 27 | #define SERIAL_PORT 0 28 | #endif 29 | 30 | // The presence of the UBRRH register is used to detect a UART. 31 | #define UART_PRESENT(port) ((port == 0 && (defined(UBRRH) || defined(UBRR0H))) || \ 32 | (port == 1 && defined(UBRR1H)) || (port == 2 && defined(UBRR2H)) || \ 33 | (port == 3 && defined(UBRR3H))) 34 | 35 | // These are macros to build serial port register names for the selected SERIAL_PORT (C preprocessor 36 | // requires two levels of indirection to expand macro values properly) 37 | #define SERIAL_REGNAME(registerbase,number,suffix) SERIAL_REGNAME_INTERNAL(registerbase,number,suffix) 38 | #if SERIAL_PORT == 0 && (!defined(UBRR0H) || !defined(UDR0)) // use un-numbered registers if necessary 39 | #define SERIAL_REGNAME_INTERNAL(registerbase,number,suffix) registerbase##suffix 40 | #else 41 | #define SERIAL_REGNAME_INTERNAL(registerbase,number,suffix) registerbase##number##suffix 42 | #endif 43 | 44 | // Registers used by MarlinSerial class (these are expanded 45 | // depending on selected serial port 46 | #define M_UCSRxA SERIAL_REGNAME(UCSR,SERIAL_PORT,A) // defines M_UCSRxA to be UCSRnA where n is the serial port number 47 | #define M_UCSRxB SERIAL_REGNAME(UCSR,SERIAL_PORT,B) 48 | #define M_RXENx SERIAL_REGNAME(RXEN,SERIAL_PORT,) 49 | #define M_TXENx SERIAL_REGNAME(TXEN,SERIAL_PORT,) 50 | #define M_RXCIEx SERIAL_REGNAME(RXCIE,SERIAL_PORT,) 51 | #define M_UDREx SERIAL_REGNAME(UDRE,SERIAL_PORT,) 52 | #define M_UDRx SERIAL_REGNAME(UDR,SERIAL_PORT,) 53 | #define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H) 54 | #define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L) 55 | #define M_RXCx SERIAL_REGNAME(RXC,SERIAL_PORT,) 56 | #define M_USARTx_RX_vect SERIAL_REGNAME(USART,SERIAL_PORT,_RX_vect) 57 | #define M_U2Xx SERIAL_REGNAME(U2X,SERIAL_PORT,) 58 | 59 | 60 | 61 | #define DEC 10 62 | #define HEX 16 63 | #define OCT 8 64 | #define BIN 2 65 | #define BYTE 0 66 | 67 | 68 | #ifndef AT90USB 69 | // Define constants and variables for buffering incoming serial data. We're 70 | // using a ring buffer (I think), in which rx_buffer_head is the index of the 71 | // location to which to write the next incoming character and rx_buffer_tail 72 | // is the index of the location from which to read. 73 | #define RX_BUFFER_SIZE 128 74 | 75 | 76 | struct ring_buffer 77 | { 78 | unsigned char buffer[RX_BUFFER_SIZE]; 79 | int head; 80 | int tail; 81 | }; 82 | 83 | #if UART_PRESENT(SERIAL_PORT) 84 | extern ring_buffer rx_buffer; 85 | #endif 86 | 87 | class MarlinSerial //: public Stream 88 | { 89 | 90 | public: 91 | MarlinSerial(); 92 | void begin(long); 93 | void end(); 94 | int peek(void); 95 | int read(void); 96 | void flush(void); 97 | 98 | FORCE_INLINE int available(void) 99 | { 100 | return (unsigned int)(RX_BUFFER_SIZE + rx_buffer.head - rx_buffer.tail) % RX_BUFFER_SIZE; 101 | } 102 | 103 | FORCE_INLINE void write(uint8_t c) 104 | { 105 | while (!((M_UCSRxA) & (1 << M_UDREx))) 106 | ; 107 | 108 | M_UDRx = c; 109 | } 110 | 111 | 112 | FORCE_INLINE void checkRx(void) 113 | { 114 | if((M_UCSRxA & (1<. 20 | */ 21 | 22 | #include "Marlin.h" 23 | #include "stepper.h" 24 | #include "planner.h" 25 | 26 | // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 27 | // segment is configured in settings.mm_per_arc_segment. 28 | void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1, 29 | uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder) 30 | { 31 | // int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); 32 | // plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc 33 | float center_axis0 = position[axis_0] + offset[axis_0]; 34 | float center_axis1 = position[axis_1] + offset[axis_1]; 35 | float linear_travel = target[axis_linear] - position[axis_linear]; 36 | float extruder_travel = target[E_AXIS] - position[E_AXIS]; 37 | float r_axis0 = -offset[axis_0]; // Radius vector from center to current location 38 | float r_axis1 = -offset[axis_1]; 39 | float rt_axis0 = target[axis_0] - center_axis0; 40 | float rt_axis1 = target[axis_1] - center_axis1; 41 | 42 | // CCW angle between position and target from circle center. Only one atan2() trig computation required. 43 | float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); 44 | if (angular_travel < 0) { angular_travel += 2*M_PI; } 45 | if (isclockwise) { angular_travel -= 2*M_PI; } 46 | 47 | //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving 48 | //to compensate when start pos = target pos && angle is zero -> angle = 2Pi 49 | if (position[axis_0] == target[axis_0] && position[axis_1] == target[axis_1] && angular_travel == 0) 50 | { 51 | angular_travel += 2*M_PI; 52 | } 53 | //end fix G03 54 | 55 | float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); 56 | if (millimeters_of_travel < 0.001) { return; } 57 | uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT); 58 | if(segments == 0) segments = 1; 59 | 60 | /* 61 | // Multiply inverse feed_rate to compensate for the fact that this movement is approximated 62 | // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 63 | // all segments. 64 | if (invert_feed_rate) { feed_rate *= segments; } 65 | */ 66 | float theta_per_segment = angular_travel/segments; 67 | float linear_per_segment = linear_travel/segments; 68 | float extruder_per_segment = extruder_travel/segments; 69 | 70 | /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, 71 | and phi is the angle of rotation. Based on the solution approach by Jens Geisler. 72 | r_T = [cos(phi) -sin(phi); 73 | sin(phi) cos(phi] * r ; 74 | 75 | For arc generation, the center of the circle is the axis of rotation and the radius vector is 76 | defined from the circle center to the initial position. Each line segment is formed by successive 77 | vector rotations. This requires only two cos() and sin() computations to form the rotation 78 | matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since 79 | all double numbers are single precision on the Arduino. (True double precision will not have 80 | round off issues for CNC applications.) Single precision error can accumulate to be greater than 81 | tool precision in some cases. Therefore, arc path correction is implemented. 82 | 83 | Small angle approximation may be used to reduce computation overhead further. This approximation 84 | holds for everything, but very small circles and large mm_per_arc_segment values. In other words, 85 | theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large 86 | to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for 87 | numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an 88 | issue for CNC machines with the single precision Arduino calculations. 89 | 90 | This approximation also allows mc_arc to immediately insert a line segment into the planner 91 | without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied 92 | a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 93 | This is important when there are successive arc motions. 94 | */ 95 | // Vector rotation matrix values 96 | float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation 97 | float sin_T = theta_per_segment; 98 | 99 | float arc_target[4]; 100 | float sin_Ti; 101 | float cos_Ti; 102 | float r_axisi; 103 | uint16_t i; 104 | int8_t count = 0; 105 | 106 | // Initialize the linear axis 107 | arc_target[axis_linear] = position[axis_linear]; 108 | 109 | // Initialize the extruder axis 110 | arc_target[E_AXIS] = position[E_AXIS]; 111 | 112 | for (i = 1; i. 19 | */ 20 | 21 | // This module is to be considered a sub-module of stepper.c. Please don't include 22 | // this file from any other module. 23 | 24 | #ifndef planner_h 25 | #define planner_h 26 | 27 | #include "Marlin.h" 28 | 29 | #ifdef ENABLE_AUTO_BED_LEVELING 30 | #include "vector_3.h" 31 | #endif // ENABLE_AUTO_BED_LEVELING 32 | 33 | // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 34 | // the source g-code and may never actually be reached if acceleration management is active. 35 | typedef struct { 36 | // Fields used by the bresenham algorithm for tracing the line 37 | long steps_x, steps_y, steps_z, steps_e; // Step count along each axis 38 | unsigned long step_event_count; // The number of step events required to complete this block 39 | long accelerate_until; // The index of the step event on which to stop acceleration 40 | long decelerate_after; // The index of the step event on which to start decelerating 41 | long acceleration_rate; // The acceleration rate used for acceleration calculation 42 | unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) 43 | unsigned char active_extruder; // Selects the active extruder 44 | #ifdef ADVANCE 45 | long advance_rate; 46 | volatile long initial_advance; 47 | volatile long final_advance; 48 | float advance; 49 | #endif 50 | 51 | // Fields used by the motion planner to manage acceleration 52 | // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis 53 | float nominal_speed; // The nominal speed for this block in mm/sec 54 | float entry_speed; // Entry speed at previous-current junction in mm/sec 55 | float max_entry_speed; // Maximum allowable junction entry speed in mm/sec 56 | float millimeters; // The total travel of this block in mm 57 | float acceleration; // acceleration mm/sec^2 58 | unsigned char recalculate_flag; // Planner flag to recalculate trapezoids on entry junction 59 | unsigned char nominal_length_flag; // Planner flag for nominal speed always reached 60 | 61 | // Settings for the trapezoid generator 62 | unsigned long nominal_rate; // The nominal step rate for this block in step_events/sec 63 | unsigned long initial_rate; // The jerk-adjusted step rate at start of block 64 | unsigned long final_rate; // The minimal rate at exit 65 | unsigned long acceleration_st; // acceleration steps/sec^2 66 | unsigned long fan_speed; 67 | #ifdef BARICUDA 68 | unsigned long valve_pressure; 69 | unsigned long e_to_p_pressure; 70 | #endif 71 | volatile char busy; 72 | } block_t; 73 | 74 | #ifdef ENABLE_AUTO_BED_LEVELING 75 | // this holds the required transform to compensate for bed level 76 | extern matrix_3x3 plan_bed_level_matrix; 77 | #endif // #ifdef ENABLE_AUTO_BED_LEVELING 78 | 79 | // Initialize the motion plan subsystem 80 | void plan_init(); 81 | 82 | // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 83 | // millimaters. Feed rate specifies the speed of the motion. 84 | 85 | #ifdef ENABLE_AUTO_BED_LEVELING 86 | void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder); 87 | 88 | // Get the position applying the bed level matrix if enabled 89 | vector_3 plan_get_position(); 90 | #else 91 | void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); 92 | #endif // ENABLE_AUTO_BED_LEVELING 93 | 94 | // Set position. Used for G92 instructions. 95 | #ifdef ENABLE_AUTO_BED_LEVELING 96 | void plan_set_position(float x, float y, float z, const float &e); 97 | #else 98 | void plan_set_position(const float &x, const float &y, const float &z, const float &e); 99 | #endif // ENABLE_AUTO_BED_LEVELING 100 | 101 | void plan_set_e_position(const float &e); 102 | 103 | 104 | 105 | void check_axes_activity(); 106 | uint8_t movesplanned(); //return the nr of buffered moves 107 | 108 | extern unsigned long minsegmenttime; 109 | extern float max_feedrate[4]; // set the max speeds 110 | extern float axis_steps_per_unit[4]; 111 | extern unsigned long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software 112 | extern float minimumfeedrate; 113 | extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX 114 | extern float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX 115 | extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. 116 | extern float max_z_jerk; 117 | extern float max_e_jerk; 118 | extern float mintravelfeedrate; 119 | extern unsigned long axis_steps_per_sqr_second[NUM_AXIS]; 120 | 121 | #ifdef AUTOTEMP 122 | extern bool autotemp_enabled; 123 | extern float autotemp_max; 124 | extern float autotemp_min; 125 | extern float autotemp_factor; 126 | #endif 127 | 128 | 129 | 130 | 131 | extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions 132 | extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed 133 | extern volatile unsigned char block_buffer_tail; 134 | // Called when the current block is no longer needed. Discards the block and makes the memory 135 | // availible for new blocks. 136 | FORCE_INLINE void plan_discard_current_block() 137 | { 138 | if (block_buffer_head != block_buffer_tail) { 139 | block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1); 140 | } 141 | } 142 | 143 | // Gets the current block. Returns NULL if buffer empty 144 | FORCE_INLINE block_t *plan_get_current_block() 145 | { 146 | if (block_buffer_head == block_buffer_tail) { 147 | return(NULL); 148 | } 149 | block_t *block = &block_buffer[block_buffer_tail]; 150 | block->busy = true; 151 | return(block); 152 | } 153 | 154 | // Returns true if the buffer has a queued block, false otherwise 155 | FORCE_INLINE bool blocks_queued() 156 | { 157 | if (block_buffer_head == block_buffer_tail) { 158 | return false; 159 | } 160 | else 161 | return true; 162 | } 163 | 164 | #ifdef PREVENT_DANGEROUS_EXTRUDE 165 | void set_extrude_min_temp(float temp); 166 | #endif 167 | 168 | void reset_acceleration_rates(); 169 | #endif 170 | -------------------------------------------------------------------------------- /MarlinSerial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | HardwareSerial.cpp - Hardware serial library for Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 23 November 2006 by David A. Mellis 20 | Modified 28 September 2010 by Mark Sproul 21 | */ 22 | 23 | #include "Marlin.h" 24 | #include "MarlinSerial.h" 25 | 26 | #ifndef AT90USB 27 | // this next line disables the entire HardwareSerial.cpp, 28 | // this is so I can support Attiny series and any other chip without a UART 29 | #if defined(UBRRH) || defined(UBRR0H) || defined(UBRR1H) || defined(UBRR2H) || defined(UBRR3H) 30 | 31 | #if UART_PRESENT(SERIAL_PORT) 32 | ring_buffer rx_buffer = { { 0 }, 0, 0 }; 33 | #endif 34 | 35 | FORCE_INLINE void store_char(unsigned char c) 36 | { 37 | int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE; 38 | 39 | // if we should be storing the received character into the location 40 | // just before the tail (meaning that the head would advance to the 41 | // current location of the tail), we're about to overflow the buffer 42 | // and so we don't write the character or advance the head. 43 | if (i != rx_buffer.tail) { 44 | rx_buffer.buffer[rx_buffer.head] = c; 45 | rx_buffer.head = i; 46 | } 47 | } 48 | 49 | 50 | //#elif defined(SIG_USART_RECV) 51 | #if defined(M_USARTx_RX_vect) 52 | // fixed by Mark Sproul this is on the 644/644p 53 | //SIGNAL(SIG_USART_RECV) 54 | SIGNAL(M_USARTx_RX_vect) 55 | { 56 | unsigned char c = M_UDRx; 57 | store_char(c); 58 | } 59 | #endif 60 | 61 | // Constructors //////////////////////////////////////////////////////////////// 62 | 63 | MarlinSerial::MarlinSerial() 64 | { 65 | 66 | } 67 | 68 | // Public Methods ////////////////////////////////////////////////////////////// 69 | 70 | void MarlinSerial::begin(long baud) 71 | { 72 | uint16_t baud_setting; 73 | bool useU2X = true; 74 | 75 | #if F_CPU == 16000000UL && SERIAL_PORT == 0 76 | // hard-coded exception for compatibility with the bootloader shipped 77 | // with the Duemilanove and previous boards and the firmware on the 8U2 78 | // on the Uno and Mega 2560. 79 | if (baud == 57600) { 80 | useU2X = false; 81 | } 82 | #endif 83 | 84 | if (useU2X) { 85 | M_UCSRxA = 1 << M_U2Xx; 86 | baud_setting = (F_CPU / 4 / baud - 1) / 2; 87 | } else { 88 | M_UCSRxA = 0; 89 | baud_setting = (F_CPU / 8 / baud - 1) / 2; 90 | } 91 | 92 | // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) 93 | M_UBRRxH = baud_setting >> 8; 94 | M_UBRRxL = baud_setting; 95 | 96 | sbi(M_UCSRxB, M_RXENx); 97 | sbi(M_UCSRxB, M_TXENx); 98 | sbi(M_UCSRxB, M_RXCIEx); 99 | } 100 | 101 | void MarlinSerial::end() 102 | { 103 | cbi(M_UCSRxB, M_RXENx); 104 | cbi(M_UCSRxB, M_TXENx); 105 | cbi(M_UCSRxB, M_RXCIEx); 106 | } 107 | 108 | 109 | 110 | int MarlinSerial::peek(void) 111 | { 112 | if (rx_buffer.head == rx_buffer.tail) { 113 | return -1; 114 | } else { 115 | return rx_buffer.buffer[rx_buffer.tail]; 116 | } 117 | } 118 | 119 | int MarlinSerial::read(void) 120 | { 121 | // if the head isn't ahead of the tail, we don't have any characters 122 | if (rx_buffer.head == rx_buffer.tail) { 123 | return -1; 124 | } else { 125 | unsigned char c = rx_buffer.buffer[rx_buffer.tail]; 126 | rx_buffer.tail = (unsigned int)(rx_buffer.tail + 1) % RX_BUFFER_SIZE; 127 | return c; 128 | } 129 | } 130 | 131 | void MarlinSerial::flush() 132 | { 133 | // don't reverse this or there may be problems if the RX interrupt 134 | // occurs after reading the value of rx_buffer_head but before writing 135 | // the value to rx_buffer_tail; the previous value of rx_buffer_head 136 | // may be written to rx_buffer_tail, making it appear as if the buffer 137 | // don't reverse this or there may be problems if the RX interrupt 138 | // occurs after reading the value of rx_buffer_head but before writing 139 | // the value to rx_buffer_tail; the previous value of rx_buffer_head 140 | // may be written to rx_buffer_tail, making it appear as if the buffer 141 | // were full, not empty. 142 | rx_buffer.head = rx_buffer.tail; 143 | } 144 | 145 | 146 | 147 | 148 | /// imports from print.h 149 | 150 | 151 | 152 | 153 | void MarlinSerial::print(char c, int base) 154 | { 155 | print((long) c, base); 156 | } 157 | 158 | void MarlinSerial::print(unsigned char b, int base) 159 | { 160 | print((unsigned long) b, base); 161 | } 162 | 163 | void MarlinSerial::print(int n, int base) 164 | { 165 | print((long) n, base); 166 | } 167 | 168 | void MarlinSerial::print(unsigned int n, int base) 169 | { 170 | print((unsigned long) n, base); 171 | } 172 | 173 | void MarlinSerial::print(long n, int base) 174 | { 175 | if (base == 0) { 176 | write(n); 177 | } else if (base == 10) { 178 | if (n < 0) { 179 | print('-'); 180 | n = -n; 181 | } 182 | printNumber(n, 10); 183 | } else { 184 | printNumber(n, base); 185 | } 186 | } 187 | 188 | void MarlinSerial::print(unsigned long n, int base) 189 | { 190 | if (base == 0) write(n); 191 | else printNumber(n, base); 192 | } 193 | 194 | void MarlinSerial::print(double n, int digits) 195 | { 196 | printFloat(n, digits); 197 | } 198 | 199 | void MarlinSerial::println(void) 200 | { 201 | print('\r'); 202 | print('\n'); 203 | } 204 | 205 | void MarlinSerial::println(const String &s) 206 | { 207 | print(s); 208 | println(); 209 | } 210 | 211 | void MarlinSerial::println(const char c[]) 212 | { 213 | print(c); 214 | println(); 215 | } 216 | 217 | void MarlinSerial::println(char c, int base) 218 | { 219 | print(c, base); 220 | println(); 221 | } 222 | 223 | void MarlinSerial::println(unsigned char b, int base) 224 | { 225 | print(b, base); 226 | println(); 227 | } 228 | 229 | void MarlinSerial::println(int n, int base) 230 | { 231 | print(n, base); 232 | println(); 233 | } 234 | 235 | void MarlinSerial::println(unsigned int n, int base) 236 | { 237 | print(n, base); 238 | println(); 239 | } 240 | 241 | void MarlinSerial::println(long n, int base) 242 | { 243 | print(n, base); 244 | println(); 245 | } 246 | 247 | void MarlinSerial::println(unsigned long n, int base) 248 | { 249 | print(n, base); 250 | println(); 251 | } 252 | 253 | void MarlinSerial::println(double n, int digits) 254 | { 255 | print(n, digits); 256 | println(); 257 | } 258 | 259 | // Private Methods ///////////////////////////////////////////////////////////// 260 | 261 | void MarlinSerial::printNumber(unsigned long n, uint8_t base) 262 | { 263 | unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. 264 | unsigned long i = 0; 265 | 266 | if (n == 0) { 267 | print('0'); 268 | return; 269 | } 270 | 271 | while (n > 0) { 272 | buf[i++] = n % base; 273 | n /= base; 274 | } 275 | 276 | for (; i > 0; i--) 277 | print((char) (buf[i - 1] < 10 ? 278 | '0' + buf[i - 1] : 279 | 'A' + buf[i - 1] - 10)); 280 | } 281 | 282 | void MarlinSerial::printFloat(double number, uint8_t digits) 283 | { 284 | // Handle negative numbers 285 | if (number < 0.0) 286 | { 287 | print('-'); 288 | number = -number; 289 | } 290 | 291 | // Round correctly so that print(1.999, 2) prints as "2.00" 292 | double rounding = 0.5; 293 | for (uint8_t i=0; i 0) 305 | print("."); 306 | 307 | // Extract digits from the remainder one at a time 308 | while (digits-- > 0) 309 | { 310 | remainder *= 10.0; 311 | int toPrint = int(remainder); 312 | print(toPrint); 313 | remainder -= toPrint; 314 | } 315 | } 316 | // Preinstantiate Objects ////////////////////////////////////////////////////// 317 | 318 | 319 | MarlinSerial MSerial; 320 | 321 | #endif // whole file 322 | #endif // !AT90USB 323 | 324 | // For AT90USB targets use the UART for BT interfacing 325 | #if defined(AT90USB) && defined (BTENABLED) 326 | HardwareSerial bt; 327 | #endif 328 | 329 | -------------------------------------------------------------------------------- /AjgHardwareSerial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | HardwareSerial.cpp - Hardware serial library for Wiring 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | Modified 23 November 2006 by David A. Mellis 20 | Modified 28 September 2010 by Mark Sproul 21 | Modified 14 August 2012 by Alarus 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "Arduino.h" 29 | #include "wiring_private.h" 30 | 31 | // this next line disables the entire HardwareSerial.cpp, 32 | // this is so I can support Attiny series and any other chip without a uart 33 | #if defined(UBRR3H) 34 | 35 | #include "AjgHardwareSerial.h" 36 | 37 | // Define constants and variables for buffering incoming serial data. We're 38 | // using a ring buffer (I think), in which head is the index of the location 39 | // to which to write the next incoming character and tail is the index of the 40 | // location from which to read. 41 | #if (RAMEND < 1000) 42 | #define SERIAL_BUFFER_SIZE 16 43 | #else 44 | #define SERIAL_BUFFER_SIZE 64 45 | #endif 46 | 47 | struct ring_buffer 48 | { 49 | unsigned char buffer[SERIAL_BUFFER_SIZE]; 50 | volatile unsigned int head; 51 | volatile unsigned int tail; 52 | }; 53 | 54 | #if defined(UBRR3H) 55 | ring_buffer rx_buffer_ajg = { { 0 }, 0, 0 }; 56 | ring_buffer tx_buffer_ajg = { { 0 }, 0, 0 }; 57 | #endif 58 | 59 | inline void store_char(unsigned char c, ring_buffer *buffer) 60 | { 61 | int i = (unsigned int)(buffer->head + 1) % SERIAL_BUFFER_SIZE; 62 | 63 | // if we should be storing the received character into the location 64 | // just before the tail (meaning that the head would advance to the 65 | // current location of the tail), we're about to overflow the buffer 66 | // and so we don't write the character or advance the head. 67 | if (i != buffer->tail) { 68 | buffer->buffer[buffer->head] = c; 69 | buffer->head = i; 70 | } 71 | } 72 | 73 | #if defined(USART3_RX_vect) && defined(UDR3) 74 | void serialEvent3() __attribute__((weak)); 75 | void serialEvent3() {} 76 | #define serialEvent3_implemented 77 | ISR(USART3_RX_vect) 78 | { 79 | if (bit_is_clear(UCSR3A, UPE3)) { 80 | unsigned char c = UDR3; 81 | store_char(c, &rx_buffer_ajg); 82 | } else { 83 | unsigned char c = UDR3; 84 | }; 85 | } 86 | #endif 87 | 88 | #ifdef USART3_UDRE_vect 89 | ISR(USART3_UDRE_vect) 90 | { 91 | if (tx_buffer_ajg.head == tx_buffer_ajg.tail) { 92 | // Buffer empty, so disable interrupts 93 | cbi(UCSR3B, UDRIE3); 94 | } 95 | else { 96 | // There is more data in the output buffer. Send the next byte 97 | unsigned char c = tx_buffer_ajg.buffer[tx_buffer_ajg.tail]; 98 | tx_buffer_ajg.tail = (tx_buffer_ajg.tail + 1) % SERIAL_BUFFER_SIZE; 99 | 100 | UDR3 = c; 101 | } 102 | } 103 | #endif 104 | 105 | 106 | // Constructors //////////////////////////////////////////////////////////////// 107 | 108 | AjgHardwareSerial::AjgHardwareSerial(ring_buffer *rx_buffer, ring_buffer *tx_buffer, 109 | volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, 110 | volatile uint8_t *ucsra, volatile uint8_t *ucsrb, 111 | volatile uint8_t *ucsrc, volatile uint8_t *udr, 112 | uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udrie, uint8_t u2x) 113 | { 114 | _rx_buffer = rx_buffer; 115 | _tx_buffer = tx_buffer; 116 | _ubrrh = ubrrh; 117 | _ubrrl = ubrrl; 118 | _ucsra = ucsra; 119 | _ucsrb = ucsrb; 120 | _ucsrc = ucsrc; 121 | _udr = udr; 122 | _rxen = rxen; 123 | _txen = txen; 124 | _rxcie = rxcie; 125 | _udrie = udrie; 126 | _u2x = u2x; 127 | } 128 | 129 | // Public Methods ////////////////////////////////////////////////////////////// 130 | 131 | void AjgHardwareSerial::begin(unsigned long baud) 132 | { 133 | uint16_t baud_setting; 134 | bool use_u2x = true; 135 | 136 | #if F_CPU == 16000000UL 137 | // hardcoded exception for compatibility with the bootloader shipped 138 | // with the Duemilanove and previous boards and the firmware on the 8U2 139 | // on the Uno and Mega 2560. 140 | if (baud == 57600) { 141 | use_u2x = false; 142 | } 143 | #endif 144 | 145 | try_again: 146 | 147 | if (use_u2x) { 148 | *_ucsra = 1 << _u2x; 149 | baud_setting = (F_CPU / 4 / baud - 1) / 2; 150 | } else { 151 | *_ucsra = 0; 152 | baud_setting = (F_CPU / 8 / baud - 1) / 2; 153 | } 154 | 155 | if ((baud_setting > 4095) && use_u2x) 156 | { 157 | use_u2x = false; 158 | goto try_again; 159 | } 160 | 161 | // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) 162 | *_ubrrh = baud_setting >> 8; 163 | *_ubrrl = baud_setting; 164 | 165 | transmitting = false; 166 | 167 | sbi(*_ucsrb, _rxen); 168 | sbi(*_ucsrb, _txen); 169 | sbi(*_ucsrb, _rxcie); 170 | cbi(*_ucsrb, _udrie); 171 | } 172 | 173 | void AjgHardwareSerial::begin(unsigned long baud, byte config) 174 | { 175 | uint16_t baud_setting; 176 | uint8_t current_config; 177 | bool use_u2x = true; 178 | 179 | #if F_CPU == 16000000UL 180 | // hardcoded exception for compatibility with the bootloader shipped 181 | // with the Duemilanove and previous boards and the firmware on the 8U2 182 | // on the Uno and Mega 2560. 183 | if (baud == 57600) { 184 | use_u2x = false; 185 | } 186 | #endif 187 | 188 | try_again: 189 | 190 | if (use_u2x) { 191 | *_ucsra = 1 << _u2x; 192 | baud_setting = (F_CPU / 4 / baud - 1) / 2; 193 | } else { 194 | *_ucsra = 0; 195 | baud_setting = (F_CPU / 8 / baud - 1) / 2; 196 | } 197 | 198 | if ((baud_setting > 4095) && use_u2x) 199 | { 200 | use_u2x = false; 201 | goto try_again; 202 | } 203 | 204 | // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) 205 | *_ubrrh = baud_setting >> 8; 206 | *_ubrrl = baud_setting; 207 | 208 | //set the data bits, parity, and stop bits 209 | #if defined(__AVR_ATmega8__) 210 | config |= 0x80; // select UCSRC register (shared with UBRRH) 211 | #endif 212 | *_ucsrc = config; 213 | 214 | sbi(*_ucsrb, _rxen); 215 | sbi(*_ucsrb, _txen); 216 | sbi(*_ucsrb, _rxcie); 217 | cbi(*_ucsrb, _udrie); 218 | } 219 | 220 | void AjgHardwareSerial::end() 221 | { 222 | // wait for transmission of outgoing data 223 | while (_tx_buffer->head != _tx_buffer->tail) 224 | ; 225 | 226 | cbi(*_ucsrb, _rxen); 227 | cbi(*_ucsrb, _txen); 228 | cbi(*_ucsrb, _rxcie); 229 | cbi(*_ucsrb, _udrie); 230 | 231 | // clear any received data 232 | _rx_buffer->head = _rx_buffer->tail; 233 | } 234 | 235 | int AjgHardwareSerial::available(void) 236 | { 237 | return (int)(SERIAL_BUFFER_SIZE + _rx_buffer->head - _rx_buffer->tail) % SERIAL_BUFFER_SIZE; 238 | } 239 | 240 | int AjgHardwareSerial::peek(void) 241 | { 242 | if (_rx_buffer->head == _rx_buffer->tail) { 243 | return -1; 244 | } else { 245 | return _rx_buffer->buffer[_rx_buffer->tail]; 246 | } 247 | } 248 | 249 | int AjgHardwareSerial::read(void) 250 | { 251 | // if the head isn't ahead of the tail, we don't have any characters 252 | if (_rx_buffer->head == _rx_buffer->tail) { 253 | return -1; 254 | } else { 255 | unsigned char c = _rx_buffer->buffer[_rx_buffer->tail]; 256 | _rx_buffer->tail = (unsigned int)(_rx_buffer->tail + 1) % SERIAL_BUFFER_SIZE; 257 | return c; 258 | } 259 | } 260 | 261 | void AjgHardwareSerial::flush() 262 | { 263 | // UDR is kept full while the buffer is not empty, so TXC triggers when EMPTY && SENT 264 | while (transmitting && ! (*_ucsra & _BV(TXC0))); 265 | transmitting = false; 266 | } 267 | 268 | size_t AjgHardwareSerial::write(uint8_t c) 269 | { 270 | int i = (_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE; 271 | 272 | // If the output buffer is full, there's nothing for it other than to 273 | // wait for the interrupt handler to empty it a bit 274 | // ???: return 0 here instead? 275 | while (i == _tx_buffer->tail) 276 | ; 277 | 278 | _tx_buffer->buffer[_tx_buffer->head] = c; 279 | _tx_buffer->head = i; 280 | 281 | sbi(*_ucsrb, _udrie); 282 | // clear the TXC bit -- "can be cleared by writing a one to its bit location" 283 | transmitting = true; 284 | sbi(*_ucsra, TXC0); 285 | 286 | return 1; 287 | } 288 | 289 | AjgHardwareSerial::operator bool() { 290 | return true; 291 | } 292 | 293 | // Preinstantiate Objects ////////////////////////////////////////////////////// 294 | 295 | #if defined(UBRR3H) 296 | AjgHardwareSerial AjgSerial(&rx_buffer_ajg, &tx_buffer_ajg, &UBRR3H, &UBRR3L, &UCSR3A, &UCSR3B, &UCSR3C, &UDR3, RXEN3, TXEN3, RXCIE3, UDRIE3, U2X3); 297 | #endif 298 | 299 | #endif // whole file 300 | 301 | -------------------------------------------------------------------------------- /SdVolume.h: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | #ifdef SDSUPPORT 22 | #ifndef SdVolume_h 23 | #define SdVolume_h 24 | /** 25 | * \file 26 | * \brief SdVolume class 27 | */ 28 | #include "SdFatConfig.h" 29 | #include "Sd2Card.h" 30 | #include "SdFatStructs.h" 31 | 32 | //============================================================================== 33 | // SdVolume class 34 | /** 35 | * \brief Cache for an SD data block 36 | */ 37 | union cache_t { 38 | /** Used to access cached file data blocks. */ 39 | uint8_t data[512]; 40 | /** Used to access cached FAT16 entries. */ 41 | uint16_t fat16[256]; 42 | /** Used to access cached FAT32 entries. */ 43 | uint32_t fat32[128]; 44 | /** Used to access cached directory entries. */ 45 | dir_t dir[16]; 46 | /** Used to access a cached Master Boot Record. */ 47 | mbr_t mbr; 48 | /** Used to access to a cached FAT boot sector. */ 49 | fat_boot_t fbs; 50 | /** Used to access to a cached FAT32 boot sector. */ 51 | fat32_boot_t fbs32; 52 | /** Used to access to a cached FAT32 FSINFO sector. */ 53 | fat32_fsinfo_t fsinfo; 54 | }; 55 | //------------------------------------------------------------------------------ 56 | /** 57 | * \class SdVolume 58 | * \brief Access FAT16 and FAT32 volumes on SD and SDHC cards. 59 | */ 60 | class SdVolume { 61 | public: 62 | /** Create an instance of SdVolume */ 63 | SdVolume() : fatType_(0) {} 64 | /** Clear the cache and returns a pointer to the cache. Used by the WaveRP 65 | * recorder to do raw write to the SD card. Not for normal apps. 66 | * \return A pointer to the cache buffer or zero if an error occurs. 67 | */ 68 | cache_t* cacheClear() { 69 | if (!cacheFlush()) return 0; 70 | cacheBlockNumber_ = 0XFFFFFFFF; 71 | return &cacheBuffer_; 72 | } 73 | /** Initialize a FAT volume. Try partition one first then try super 74 | * floppy format. 75 | * 76 | * \param[in] dev The Sd2Card where the volume is located. 77 | * 78 | * \return The value one, true, is returned for success and 79 | * the value zero, false, is returned for failure. Reasons for 80 | * failure include not finding a valid partition, not finding a valid 81 | * FAT file system or an I/O error. 82 | */ 83 | bool init(Sd2Card* dev) { return init(dev, 1) ? true : init(dev, 0);} 84 | bool init(Sd2Card* dev, uint8_t part); 85 | 86 | // inline functions that return volume info 87 | /** \return The volume's cluster size in blocks. */ 88 | uint8_t blocksPerCluster() const {return blocksPerCluster_;} 89 | /** \return The number of blocks in one FAT. */ 90 | uint32_t blocksPerFat() const {return blocksPerFat_;} 91 | /** \return The total number of clusters in the volume. */ 92 | uint32_t clusterCount() const {return clusterCount_;} 93 | /** \return The shift count required to multiply by blocksPerCluster. */ 94 | uint8_t clusterSizeShift() const {return clusterSizeShift_;} 95 | /** \return The logical block number for the start of file data. */ 96 | uint32_t dataStartBlock() const {return dataStartBlock_;} 97 | /** \return The number of FAT structures on the volume. */ 98 | uint8_t fatCount() const {return fatCount_;} 99 | /** \return The logical block number for the start of the first FAT. */ 100 | uint32_t fatStartBlock() const {return fatStartBlock_;} 101 | /** \return The FAT type of the volume. Values are 12, 16 or 32. */ 102 | uint8_t fatType() const {return fatType_;} 103 | int32_t freeClusterCount(); 104 | /** \return The number of entries in the root directory for FAT16 volumes. */ 105 | uint32_t rootDirEntryCount() const {return rootDirEntryCount_;} 106 | /** \return The logical block number for the start of the root directory 107 | on FAT16 volumes or the first cluster number on FAT32 volumes. */ 108 | uint32_t rootDirStart() const {return rootDirStart_;} 109 | /** Sd2Card object for this volume 110 | * \return pointer to Sd2Card object. 111 | */ 112 | Sd2Card* sdCard() {return sdCard_;} 113 | /** Debug access to FAT table 114 | * 115 | * \param[in] n cluster number. 116 | * \param[out] v value of entry 117 | * \return true for success or false for failure 118 | */ 119 | bool dbgFat(uint32_t n, uint32_t* v) {return fatGet(n, v);} 120 | //------------------------------------------------------------------------------ 121 | private: 122 | // Allow SdBaseFile access to SdVolume private data. 123 | friend class SdBaseFile; 124 | 125 | // value for dirty argument in cacheRawBlock to indicate read from cache 126 | static bool const CACHE_FOR_READ = false; 127 | // value for dirty argument in cacheRawBlock to indicate write to cache 128 | static bool const CACHE_FOR_WRITE = true; 129 | 130 | #if USE_MULTIPLE_CARDS 131 | cache_t cacheBuffer_; // 512 byte cache for device blocks 132 | uint32_t cacheBlockNumber_; // Logical number of block in the cache 133 | Sd2Card* sdCard_; // Sd2Card object for cache 134 | bool cacheDirty_; // cacheFlush() will write block if true 135 | uint32_t cacheMirrorBlock_; // block number for mirror FAT 136 | #else // USE_MULTIPLE_CARDS 137 | static cache_t cacheBuffer_; // 512 byte cache for device blocks 138 | static uint32_t cacheBlockNumber_; // Logical number of block in the cache 139 | static Sd2Card* sdCard_; // Sd2Card object for cache 140 | static bool cacheDirty_; // cacheFlush() will write block if true 141 | static uint32_t cacheMirrorBlock_; // block number for mirror FAT 142 | #endif // USE_MULTIPLE_CARDS 143 | uint32_t allocSearchStart_; // start cluster for alloc search 144 | uint8_t blocksPerCluster_; // cluster size in blocks 145 | uint32_t blocksPerFat_; // FAT size in blocks 146 | uint32_t clusterCount_; // clusters in one FAT 147 | uint8_t clusterSizeShift_; // shift to convert cluster count to block count 148 | uint32_t dataStartBlock_; // first data block number 149 | uint8_t fatCount_; // number of FATs on volume 150 | uint32_t fatStartBlock_; // start block for first FAT 151 | uint8_t fatType_; // volume type (12, 16, OR 32) 152 | uint16_t rootDirEntryCount_; // number of entries in FAT16 root dir 153 | uint32_t rootDirStart_; // root start block for FAT16, cluster for FAT32 154 | //---------------------------------------------------------------------------- 155 | bool allocContiguous(uint32_t count, uint32_t* curCluster); 156 | uint8_t blockOfCluster(uint32_t position) const { 157 | return (position >> 9) & (blocksPerCluster_ - 1);} 158 | uint32_t clusterStartBlock(uint32_t cluster) const { 159 | return dataStartBlock_ + ((cluster - 2) << clusterSizeShift_);} 160 | uint32_t blockNumber(uint32_t cluster, uint32_t position) const { 161 | return clusterStartBlock(cluster) + blockOfCluster(position);} 162 | cache_t *cache() {return &cacheBuffer_;} 163 | uint32_t cacheBlockNumber() {return cacheBlockNumber_;} 164 | #if USE_MULTIPLE_CARDS 165 | bool cacheFlush(); 166 | bool cacheRawBlock(uint32_t blockNumber, bool dirty); 167 | #else // USE_MULTIPLE_CARDS 168 | static bool cacheFlush(); 169 | static bool cacheRawBlock(uint32_t blockNumber, bool dirty); 170 | #endif // USE_MULTIPLE_CARDS 171 | // used by SdBaseFile write to assign cache to SD location 172 | void cacheSetBlockNumber(uint32_t blockNumber, bool dirty) { 173 | cacheDirty_ = dirty; 174 | cacheBlockNumber_ = blockNumber; 175 | } 176 | void cacheSetDirty() {cacheDirty_ |= CACHE_FOR_WRITE;} 177 | bool chainSize(uint32_t beginCluster, uint32_t* size); 178 | bool fatGet(uint32_t cluster, uint32_t* value); 179 | bool fatPut(uint32_t cluster, uint32_t value); 180 | bool fatPutEOC(uint32_t cluster) { 181 | return fatPut(cluster, 0x0FFFFFFF); 182 | } 183 | bool freeChain(uint32_t cluster); 184 | bool isEOC(uint32_t cluster) const { 185 | if (FAT12_SUPPORT && fatType_ == 12) return cluster >= FAT12EOC_MIN; 186 | if (fatType_ == 16) return cluster >= FAT16EOC_MIN; 187 | return cluster >= FAT32EOC_MIN; 188 | } 189 | bool readBlock(uint32_t block, uint8_t* dst) { 190 | return sdCard_->readBlock(block, dst);} 191 | bool writeBlock(uint32_t block, const uint8_t* dst) { 192 | return sdCard_->writeBlock(block, dst); 193 | } 194 | //------------------------------------------------------------------------------ 195 | // Deprecated functions - suppress cpplint warnings with NOLINT comment 196 | #if ALLOW_DEPRECATED_FUNCTIONS && !defined(DOXYGEN) 197 | public: 198 | /** \deprecated Use: bool SdVolume::init(Sd2Card* dev); 199 | * \param[in] dev The SD card where the volume is located. 200 | * \return true for success or false for failure. 201 | */ 202 | bool init(Sd2Card& dev) {return init(&dev);} // NOLINT 203 | /** \deprecated Use: bool SdVolume::init(Sd2Card* dev, uint8_t vol); 204 | * \param[in] dev The SD card where the volume is located. 205 | * \param[in] part The partition to be used. 206 | * \return true for success or false for failure. 207 | */ 208 | bool init(Sd2Card& dev, uint8_t part) { // NOLINT 209 | return init(&dev, part); 210 | } 211 | #endif // ALLOW_DEPRECATED_FUNCTIONS 212 | }; 213 | #endif // SdVolume 214 | #endif 215 | -------------------------------------------------------------------------------- /Sd2Card.h: -------------------------------------------------------------------------------- 1 | /* Arduino Sd2Card Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino Sd2Card Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino Sd2Card Library. If not, see 18 | * . 19 | */ 20 | 21 | #include "Marlin.h" 22 | #ifdef SDSUPPORT 23 | 24 | #ifndef Sd2Card_h 25 | #define Sd2Card_h 26 | /** 27 | * \file 28 | * \brief Sd2Card class for V2 SD/SDHC cards 29 | */ 30 | #include "SdFatConfig.h" 31 | #include "Sd2PinMap.h" 32 | #include "SdInfo.h" 33 | //------------------------------------------------------------------------------ 34 | // SPI speed is F_CPU/2^(1 + index), 0 <= index <= 6 35 | /** Set SCK to max rate of F_CPU/2. See Sd2Card::setSckRate(). */ 36 | uint8_t const SPI_FULL_SPEED = 0; 37 | /** Set SCK rate to F_CPU/4. See Sd2Card::setSckRate(). */ 38 | uint8_t const SPI_HALF_SPEED = 1; 39 | /** Set SCK rate to F_CPU/8. See Sd2Card::setSckRate(). */ 40 | uint8_t const SPI_QUARTER_SPEED = 2; 41 | /** Set SCK rate to F_CPU/16. See Sd2Card::setSckRate(). */ 42 | uint8_t const SPI_EIGHTH_SPEED = 3; 43 | /** Set SCK rate to F_CPU/32. See Sd2Card::setSckRate(). */ 44 | uint8_t const SPI_SIXTEENTH_SPEED = 4; 45 | //------------------------------------------------------------------------------ 46 | /** init timeout ms */ 47 | uint16_t const SD_INIT_TIMEOUT = 2000; 48 | /** erase timeout ms */ 49 | uint16_t const SD_ERASE_TIMEOUT = 10000; 50 | /** read timeout ms */ 51 | uint16_t const SD_READ_TIMEOUT = 300; 52 | /** write time out ms */ 53 | uint16_t const SD_WRITE_TIMEOUT = 600; 54 | //------------------------------------------------------------------------------ 55 | // SD card errors 56 | /** timeout error for command CMD0 (initialize card in SPI mode) */ 57 | uint8_t const SD_CARD_ERROR_CMD0 = 0X1; 58 | /** CMD8 was not accepted - not a valid SD card*/ 59 | uint8_t const SD_CARD_ERROR_CMD8 = 0X2; 60 | /** card returned an error response for CMD12 (write stop) */ 61 | uint8_t const SD_CARD_ERROR_CMD12 = 0X3; 62 | /** card returned an error response for CMD17 (read block) */ 63 | uint8_t const SD_CARD_ERROR_CMD17 = 0X4; 64 | /** card returned an error response for CMD18 (read multiple block) */ 65 | uint8_t const SD_CARD_ERROR_CMD18 = 0X5; 66 | /** card returned an error response for CMD24 (write block) */ 67 | uint8_t const SD_CARD_ERROR_CMD24 = 0X6; 68 | /** WRITE_MULTIPLE_BLOCKS command failed */ 69 | uint8_t const SD_CARD_ERROR_CMD25 = 0X7; 70 | /** card returned an error response for CMD58 (read OCR) */ 71 | uint8_t const SD_CARD_ERROR_CMD58 = 0X8; 72 | /** SET_WR_BLK_ERASE_COUNT failed */ 73 | uint8_t const SD_CARD_ERROR_ACMD23 = 0X9; 74 | /** ACMD41 initialization process timeout */ 75 | uint8_t const SD_CARD_ERROR_ACMD41 = 0XA; 76 | /** card returned a bad CSR version field */ 77 | uint8_t const SD_CARD_ERROR_BAD_CSD = 0XB; 78 | /** erase block group command failed */ 79 | uint8_t const SD_CARD_ERROR_ERASE = 0XC; 80 | /** card not capable of single block erase */ 81 | uint8_t const SD_CARD_ERROR_ERASE_SINGLE_BLOCK = 0XD; 82 | /** Erase sequence timed out */ 83 | uint8_t const SD_CARD_ERROR_ERASE_TIMEOUT = 0XE; 84 | /** card returned an error token instead of read data */ 85 | uint8_t const SD_CARD_ERROR_READ = 0XF; 86 | /** read CID or CSD failed */ 87 | uint8_t const SD_CARD_ERROR_READ_REG = 0X10; 88 | /** timeout while waiting for start of read data */ 89 | uint8_t const SD_CARD_ERROR_READ_TIMEOUT = 0X11; 90 | /** card did not accept STOP_TRAN_TOKEN */ 91 | uint8_t const SD_CARD_ERROR_STOP_TRAN = 0X12; 92 | /** card returned an error token as a response to a write operation */ 93 | uint8_t const SD_CARD_ERROR_WRITE = 0X13; 94 | /** attempt to write protected block zero */ 95 | uint8_t const SD_CARD_ERROR_WRITE_BLOCK_ZERO = 0X14; // REMOVE - not used 96 | /** card did not go ready for a multiple block write */ 97 | uint8_t const SD_CARD_ERROR_WRITE_MULTIPLE = 0X15; 98 | /** card returned an error to a CMD13 status check after a write */ 99 | uint8_t const SD_CARD_ERROR_WRITE_PROGRAMMING = 0X16; 100 | /** timeout occurred during write programming */ 101 | uint8_t const SD_CARD_ERROR_WRITE_TIMEOUT = 0X17; 102 | /** incorrect rate selected */ 103 | uint8_t const SD_CARD_ERROR_SCK_RATE = 0X18; 104 | /** init() not called */ 105 | uint8_t const SD_CARD_ERROR_INIT_NOT_CALLED = 0X19; 106 | /** crc check error */ 107 | uint8_t const SD_CARD_ERROR_CRC = 0X20; 108 | //------------------------------------------------------------------------------ 109 | // card types 110 | /** Standard capacity V1 SD card */ 111 | uint8_t const SD_CARD_TYPE_SD1 = 1; 112 | /** Standard capacity V2 SD card */ 113 | uint8_t const SD_CARD_TYPE_SD2 = 2; 114 | /** High Capacity SD card */ 115 | uint8_t const SD_CARD_TYPE_SDHC = 3; 116 | /** 117 | * define SOFTWARE_SPI to use bit-bang SPI 118 | */ 119 | //------------------------------------------------------------------------------ 120 | #if MEGA_SOFT_SPI && (defined(__AVR_ATmega1280__)||defined(__AVR_ATmega2560__)) 121 | #define SOFTWARE_SPI 122 | #elif USE_SOFTWARE_SPI 123 | #define SOFTWARE_SPI 124 | #endif // MEGA_SOFT_SPI 125 | //------------------------------------------------------------------------------ 126 | // SPI pin definitions - do not edit here - change in SdFatConfig.h 127 | // 128 | #ifndef SOFTWARE_SPI 129 | // hardware pin defs 130 | /** The default chip select pin for the SD card is SS. */ 131 | uint8_t const SD_CHIP_SELECT_PIN = SS_PIN; 132 | // The following three pins must not be redefined for hardware SPI. 133 | /** SPI Master Out Slave In pin */ 134 | uint8_t const SPI_MOSI_PIN = MOSI_PIN; 135 | /** SPI Master In Slave Out pin */ 136 | uint8_t const SPI_MISO_PIN = MISO_PIN; 137 | /** SPI Clock pin */ 138 | uint8_t const SPI_SCK_PIN = SCK_PIN; 139 | 140 | #else // SOFTWARE_SPI 141 | 142 | /** SPI chip select pin */ 143 | uint8_t const SD_CHIP_SELECT_PIN = SOFT_SPI_CS_PIN; 144 | /** SPI Master Out Slave In pin */ 145 | uint8_t const SPI_MOSI_PIN = SOFT_SPI_MOSI_PIN; 146 | /** SPI Master In Slave Out pin */ 147 | uint8_t const SPI_MISO_PIN = SOFT_SPI_MISO_PIN; 148 | /** SPI Clock pin */ 149 | uint8_t const SPI_SCK_PIN = SOFT_SPI_SCK_PIN; 150 | #endif // SOFTWARE_SPI 151 | //------------------------------------------------------------------------------ 152 | /** 153 | * \class Sd2Card 154 | * \brief Raw access to SD and SDHC flash memory cards. 155 | */ 156 | class Sd2Card { 157 | public: 158 | /** Construct an instance of Sd2Card. */ 159 | Sd2Card() : errorCode_(SD_CARD_ERROR_INIT_NOT_CALLED), type_(0) {} 160 | uint32_t cardSize(); 161 | bool erase(uint32_t firstBlock, uint32_t lastBlock); 162 | bool eraseSingleBlockEnable(); 163 | /** 164 | * Set SD error code. 165 | * \param[in] code value for error code. 166 | */ 167 | void error(uint8_t code) {errorCode_ = code;} 168 | /** 169 | * \return error code for last error. See Sd2Card.h for a list of error codes. 170 | */ 171 | int errorCode() const {return errorCode_;} 172 | /** \return error data for last error. */ 173 | int errorData() const {return status_;} 174 | /** 175 | * Initialize an SD flash memory card with default clock rate and chip 176 | * select pin. See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin). 177 | * 178 | * \return true for success or false for failure. 179 | */ 180 | bool init(uint8_t sckRateID = SPI_FULL_SPEED, 181 | uint8_t chipSelectPin = SD_CHIP_SELECT_PIN); 182 | bool readBlock(uint32_t block, uint8_t* dst); 183 | /** 184 | * Read a card's CID register. The CID contains card identification 185 | * information such as Manufacturer ID, Product name, Product serial 186 | * number and Manufacturing date. 187 | * 188 | * \param[out] cid pointer to area for returned data. 189 | * 190 | * \return true for success or false for failure. 191 | */ 192 | bool readCID(cid_t* cid) { 193 | return readRegister(CMD10, cid); 194 | } 195 | /** 196 | * Read a card's CSD register. The CSD contains Card-Specific Data that 197 | * provides information regarding access to the card's contents. 198 | * 199 | * \param[out] csd pointer to area for returned data. 200 | * 201 | * \return true for success or false for failure. 202 | */ 203 | bool readCSD(csd_t* csd) { 204 | return readRegister(CMD9, csd); 205 | } 206 | bool readData(uint8_t *dst); 207 | bool readStart(uint32_t blockNumber); 208 | bool readStop(); 209 | bool setSckRate(uint8_t sckRateID); 210 | /** Return the card type: SD V1, SD V2 or SDHC 211 | * \return 0 - SD V1, 1 - SD V2, or 3 - SDHC. 212 | */ 213 | int type() const {return type_;} 214 | bool writeBlock(uint32_t blockNumber, const uint8_t* src); 215 | bool writeData(const uint8_t* src); 216 | bool writeStart(uint32_t blockNumber, uint32_t eraseCount); 217 | bool writeStop(); 218 | private: 219 | //---------------------------------------------------------------------------- 220 | uint8_t chipSelectPin_; 221 | uint8_t errorCode_; 222 | uint8_t spiRate_; 223 | uint8_t status_; 224 | uint8_t type_; 225 | // private functions 226 | uint8_t cardAcmd(uint8_t cmd, uint32_t arg) { 227 | cardCommand(CMD55, 0); 228 | return cardCommand(cmd, arg); 229 | } 230 | uint8_t cardCommand(uint8_t cmd, uint32_t arg); 231 | 232 | bool readData(uint8_t* dst, uint16_t count); 233 | bool readRegister(uint8_t cmd, void* buf); 234 | void chipSelectHigh(); 235 | void chipSelectLow(); 236 | void type(uint8_t value) {type_ = value;} 237 | bool waitNotBusy(uint16_t timeoutMillis); 238 | bool writeData(uint8_t token, const uint8_t* src); 239 | }; 240 | #endif // Sd2Card_h 241 | 242 | 243 | #endif 244 | -------------------------------------------------------------------------------- /SdInfo.h: -------------------------------------------------------------------------------- 1 | /* Arduino Sd2Card Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino Sd2Card Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino Sd2Card Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | #ifdef SDSUPPORT 22 | 23 | #ifndef SdInfo_h 24 | #define SdInfo_h 25 | #include 26 | // Based on the document: 27 | // 28 | // SD Specifications 29 | // Part 1 30 | // Physical Layer 31 | // Simplified Specification 32 | // Version 3.01 33 | // May 18, 2010 34 | // 35 | // http://www.sdcard.org/developers/tech/sdcard/pls/simplified_specs 36 | //------------------------------------------------------------------------------ 37 | // SD card commands 38 | /** GO_IDLE_STATE - init card in spi mode if CS low */ 39 | uint8_t const CMD0 = 0X00; 40 | /** SEND_IF_COND - verify SD Memory Card interface operating condition.*/ 41 | uint8_t const CMD8 = 0X08; 42 | /** SEND_CSD - read the Card Specific Data (CSD register) */ 43 | uint8_t const CMD9 = 0X09; 44 | /** SEND_CID - read the card identification information (CID register) */ 45 | uint8_t const CMD10 = 0X0A; 46 | /** STOP_TRANSMISSION - end multiple block read sequence */ 47 | uint8_t const CMD12 = 0X0C; 48 | /** SEND_STATUS - read the card status register */ 49 | uint8_t const CMD13 = 0X0D; 50 | /** READ_SINGLE_BLOCK - read a single data block from the card */ 51 | uint8_t const CMD17 = 0X11; 52 | /** READ_MULTIPLE_BLOCK - read a multiple data blocks from the card */ 53 | uint8_t const CMD18 = 0X12; 54 | /** WRITE_BLOCK - write a single data block to the card */ 55 | uint8_t const CMD24 = 0X18; 56 | /** WRITE_MULTIPLE_BLOCK - write blocks of data until a STOP_TRANSMISSION */ 57 | uint8_t const CMD25 = 0X19; 58 | /** ERASE_WR_BLK_START - sets the address of the first block to be erased */ 59 | uint8_t const CMD32 = 0X20; 60 | /** ERASE_WR_BLK_END - sets the address of the last block of the continuous 61 | range to be erased*/ 62 | uint8_t const CMD33 = 0X21; 63 | /** ERASE - erase all previously selected blocks */ 64 | uint8_t const CMD38 = 0X26; 65 | /** APP_CMD - escape for application specific command */ 66 | uint8_t const CMD55 = 0X37; 67 | /** READ_OCR - read the OCR register of a card */ 68 | uint8_t const CMD58 = 0X3A; 69 | /** SET_WR_BLK_ERASE_COUNT - Set the number of write blocks to be 70 | pre-erased before writing */ 71 | uint8_t const ACMD23 = 0X17; 72 | /** SD_SEND_OP_COMD - Sends host capacity support information and 73 | activates the card's initialization process */ 74 | uint8_t const ACMD41 = 0X29; 75 | //------------------------------------------------------------------------------ 76 | /** status for card in the ready state */ 77 | uint8_t const R1_READY_STATE = 0X00; 78 | /** status for card in the idle state */ 79 | uint8_t const R1_IDLE_STATE = 0X01; 80 | /** status bit for illegal command */ 81 | uint8_t const R1_ILLEGAL_COMMAND = 0X04; 82 | /** start data token for read or write single block*/ 83 | uint8_t const DATA_START_BLOCK = 0XFE; 84 | /** stop token for write multiple blocks*/ 85 | uint8_t const STOP_TRAN_TOKEN = 0XFD; 86 | /** start data token for write multiple blocks*/ 87 | uint8_t const WRITE_MULTIPLE_TOKEN = 0XFC; 88 | /** mask for data response tokens after a write block operation */ 89 | uint8_t const DATA_RES_MASK = 0X1F; 90 | /** write data accepted token */ 91 | uint8_t const DATA_RES_ACCEPTED = 0X05; 92 | //------------------------------------------------------------------------------ 93 | /** Card IDentification (CID) register */ 94 | typedef struct CID { 95 | // byte 0 96 | /** Manufacturer ID */ 97 | unsigned char mid; 98 | // byte 1-2 99 | /** OEM/Application ID */ 100 | char oid[2]; 101 | // byte 3-7 102 | /** Product name */ 103 | char pnm[5]; 104 | // byte 8 105 | /** Product revision least significant digit */ 106 | unsigned char prv_m : 4; 107 | /** Product revision most significant digit */ 108 | unsigned char prv_n : 4; 109 | // byte 9-12 110 | /** Product serial number */ 111 | uint32_t psn; 112 | // byte 13 113 | /** Manufacturing date year low digit */ 114 | unsigned char mdt_year_high : 4; 115 | /** not used */ 116 | unsigned char reserved : 4; 117 | // byte 14 118 | /** Manufacturing date month */ 119 | unsigned char mdt_month : 4; 120 | /** Manufacturing date year low digit */ 121 | unsigned char mdt_year_low :4; 122 | // byte 15 123 | /** not used always 1 */ 124 | unsigned char always1 : 1; 125 | /** CRC7 checksum */ 126 | unsigned char crc : 7; 127 | }cid_t; 128 | //------------------------------------------------------------------------------ 129 | /** CSD for version 1.00 cards */ 130 | typedef struct CSDV1 { 131 | // byte 0 132 | unsigned char reserved1 : 6; 133 | unsigned char csd_ver : 2; 134 | // byte 1 135 | unsigned char taac; 136 | // byte 2 137 | unsigned char nsac; 138 | // byte 3 139 | unsigned char tran_speed; 140 | // byte 4 141 | unsigned char ccc_high; 142 | // byte 5 143 | unsigned char read_bl_len : 4; 144 | unsigned char ccc_low : 4; 145 | // byte 6 146 | unsigned char c_size_high : 2; 147 | unsigned char reserved2 : 2; 148 | unsigned char dsr_imp : 1; 149 | unsigned char read_blk_misalign :1; 150 | unsigned char write_blk_misalign : 1; 151 | unsigned char read_bl_partial : 1; 152 | // byte 7 153 | unsigned char c_size_mid; 154 | // byte 8 155 | unsigned char vdd_r_curr_max : 3; 156 | unsigned char vdd_r_curr_min : 3; 157 | unsigned char c_size_low :2; 158 | // byte 9 159 | unsigned char c_size_mult_high : 2; 160 | unsigned char vdd_w_cur_max : 3; 161 | unsigned char vdd_w_curr_min : 3; 162 | // byte 10 163 | unsigned char sector_size_high : 6; 164 | unsigned char erase_blk_en : 1; 165 | unsigned char c_size_mult_low : 1; 166 | // byte 11 167 | unsigned char wp_grp_size : 7; 168 | unsigned char sector_size_low : 1; 169 | // byte 12 170 | unsigned char write_bl_len_high : 2; 171 | unsigned char r2w_factor : 3; 172 | unsigned char reserved3 : 2; 173 | unsigned char wp_grp_enable : 1; 174 | // byte 13 175 | unsigned char reserved4 : 5; 176 | unsigned char write_partial : 1; 177 | unsigned char write_bl_len_low : 2; 178 | // byte 14 179 | unsigned char reserved5: 2; 180 | unsigned char file_format : 2; 181 | unsigned char tmp_write_protect : 1; 182 | unsigned char perm_write_protect : 1; 183 | unsigned char copy : 1; 184 | /** Indicates the file format on the card */ 185 | unsigned char file_format_grp : 1; 186 | // byte 15 187 | unsigned char always1 : 1; 188 | unsigned char crc : 7; 189 | }csd1_t; 190 | //------------------------------------------------------------------------------ 191 | /** CSD for version 2.00 cards */ 192 | typedef struct CSDV2 { 193 | // byte 0 194 | unsigned char reserved1 : 6; 195 | unsigned char csd_ver : 2; 196 | // byte 1 197 | /** fixed to 0X0E */ 198 | unsigned char taac; 199 | // byte 2 200 | /** fixed to 0 */ 201 | unsigned char nsac; 202 | // byte 3 203 | unsigned char tran_speed; 204 | // byte 4 205 | unsigned char ccc_high; 206 | // byte 5 207 | /** This field is fixed to 9h, which indicates READ_BL_LEN=512 Byte */ 208 | unsigned char read_bl_len : 4; 209 | unsigned char ccc_low : 4; 210 | // byte 6 211 | /** not used */ 212 | unsigned char reserved2 : 4; 213 | unsigned char dsr_imp : 1; 214 | /** fixed to 0 */ 215 | unsigned char read_blk_misalign :1; 216 | /** fixed to 0 */ 217 | unsigned char write_blk_misalign : 1; 218 | /** fixed to 0 - no partial read */ 219 | unsigned char read_bl_partial : 1; 220 | // byte 7 221 | /** not used */ 222 | unsigned char reserved3 : 2; 223 | /** high part of card size */ 224 | unsigned char c_size_high : 6; 225 | // byte 8 226 | /** middle part of card size */ 227 | unsigned char c_size_mid; 228 | // byte 9 229 | /** low part of card size */ 230 | unsigned char c_size_low; 231 | // byte 10 232 | /** sector size is fixed at 64 KB */ 233 | unsigned char sector_size_high : 6; 234 | /** fixed to 1 - erase single is supported */ 235 | unsigned char erase_blk_en : 1; 236 | /** not used */ 237 | unsigned char reserved4 : 1; 238 | // byte 11 239 | unsigned char wp_grp_size : 7; 240 | /** sector size is fixed at 64 KB */ 241 | unsigned char sector_size_low : 1; 242 | // byte 12 243 | /** write_bl_len fixed for 512 byte blocks */ 244 | unsigned char write_bl_len_high : 2; 245 | /** fixed value of 2 */ 246 | unsigned char r2w_factor : 3; 247 | /** not used */ 248 | unsigned char reserved5 : 2; 249 | /** fixed value of 0 - no write protect groups */ 250 | unsigned char wp_grp_enable : 1; 251 | // byte 13 252 | unsigned char reserved6 : 5; 253 | /** always zero - no partial block read*/ 254 | unsigned char write_partial : 1; 255 | /** write_bl_len fixed for 512 byte blocks */ 256 | unsigned char write_bl_len_low : 2; 257 | // byte 14 258 | unsigned char reserved7: 2; 259 | /** Do not use always 0 */ 260 | unsigned char file_format : 2; 261 | unsigned char tmp_write_protect : 1; 262 | unsigned char perm_write_protect : 1; 263 | unsigned char copy : 1; 264 | /** Do not use always 0 */ 265 | unsigned char file_format_grp : 1; 266 | // byte 15 267 | /** not used always 1 */ 268 | unsigned char always1 : 1; 269 | /** checksum */ 270 | unsigned char crc : 7; 271 | }csd2_t; 272 | //------------------------------------------------------------------------------ 273 | /** union of old and new style CSD register */ 274 | union csd_t { 275 | csd1_t v1; 276 | csd2_t v2; 277 | }; 278 | #endif // SdInfo_h 279 | 280 | #endif 281 | -------------------------------------------------------------------------------- /Marlin.h: -------------------------------------------------------------------------------- 1 | // Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. 2 | // License: GPL 3 | 4 | #ifndef MARLIN_H 5 | #define MARLIN_H 6 | 7 | #define FORCE_INLINE __attribute__((always_inline)) inline 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | #include "fastio.h" 22 | #include "Configuration.h" 23 | #include "pins.h" 24 | 25 | #ifndef AT90USB 26 | #define HardwareSerial_h // trick to disable the standard HWserial 27 | #endif 28 | 29 | #if (ARDUINO >= 100) 30 | # include "Arduino.h" 31 | #else 32 | # include "WProgram.h" 33 | //Arduino < 1.0.0 does not define this, so we need to do it ourselves 34 | # define analogInputToDigitalPin(p) ((p) + A0) 35 | #endif 36 | 37 | #ifdef AT90USB 38 | #include "HardwareSerial.h" 39 | #endif 40 | 41 | #include "MarlinSerial.h" 42 | 43 | #include "AjgHardwareSerial.h" 44 | 45 | #ifndef cbi 46 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 47 | #endif 48 | #ifndef sbi 49 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 50 | #endif 51 | 52 | #include "WString.h" 53 | 54 | #ifdef AT90USB 55 | #ifdef BTENABLED 56 | #define MYSERIAL bt 57 | #else 58 | #define MYSERIAL Serial 59 | #endif // BTENABLED 60 | #else 61 | #define MYSERIAL MSerial 62 | #endif 63 | 64 | 65 | //agj serial interface,add by zrb 2015/01/17 66 | #define AJG_SERIAL_PROTOCOL(x) (AjgSerial.print(x)) 67 | #define AJG_SERIAL_PROTOCOL_F(x,y) (AjgSerial.print(x,y)) 68 | #define AJG_SERIAL_PROTOCOLPGM(x) (ajgSerialprintPGM(PSTR(x))) 69 | #define AJG_SERIAL_PROTOCOLLN(x) (AjgSerial.print(x),AjgSerial.write('\n')) 70 | #define AJG_SERIAL_PROTOCOLLNPGM(x) (ajgSerialprintPGM(PSTR(x)),AjgSerial.write('\n')) 71 | 72 | 73 | const char ajgErr[] PROGMEM ="ERR "; 74 | const char ajgSucc[] PROGMEM ="OK N"; 75 | #define AJG_SERIAL_ERROR_START (ajgSerialprintPGM(ajgErr)) 76 | #define AJG_SERIAL_ERROR(x) AJG_SERIAL_PROTOCOL(x) 77 | #define AJG_SERIAL_ERRORPGM(x) AJG_SERIAL_PROTOCOLPGM(x) 78 | #define AJG_SERIAL_ERRORLN(x) AJG_SERIAL_PROTOCOLLN(x) 79 | 80 | #define AJG_SERIAL_SUCC_START (ajgSerialprintPGM(ajgSucc)) 81 | 82 | 83 | 84 | 85 | #define SERIAL_PROTOCOL(x) (MYSERIAL.print(x)) 86 | #define SERIAL_PROTOCOL_F(x,y) (MYSERIAL.print(x,y)) 87 | #define SERIAL_PROTOCOLPGM(x) (serialprintPGM(PSTR(x))) 88 | #define SERIAL_PROTOCOLLN(x) (MYSERIAL.print(x),MYSERIAL.write('\n')) 89 | #define SERIAL_PROTOCOLLNPGM(x) (serialprintPGM(PSTR(x)),MYSERIAL.write('\n')) 90 | 91 | 92 | const char errormagic[] PROGMEM ="Error:"; 93 | const char echomagic[] PROGMEM ="echo:"; 94 | #define SERIAL_ERROR_START (serialprintPGM(errormagic)) 95 | #define SERIAL_ERROR(x) SERIAL_PROTOCOL(x) 96 | #define SERIAL_ERRORPGM(x) SERIAL_PROTOCOLPGM(x) 97 | #define SERIAL_ERRORLN(x) SERIAL_PROTOCOLLN(x) 98 | #define SERIAL_ERRORLNPGM(x) SERIAL_PROTOCOLLNPGM(x) 99 | 100 | #define SERIAL_ECHO_START (serialprintPGM(echomagic)) 101 | #define SERIAL_ECHO(x) SERIAL_PROTOCOL(x) 102 | #define SERIAL_ECHOPGM(x) SERIAL_PROTOCOLPGM(x) 103 | #define SERIAL_ECHOLN(x) SERIAL_PROTOCOLLN(x) 104 | #define SERIAL_ECHOLNPGM(x) SERIAL_PROTOCOLLNPGM(x) 105 | 106 | #define SERIAL_ECHOPAIR(name,value) (serial_echopair_P(PSTR(name),(value))) 107 | 108 | void serial_echopair_P(const char *s_P, float v); 109 | void serial_echopair_P(const char *s_P, double v); 110 | void serial_echopair_P(const char *s_P, unsigned long v); 111 | 112 | 113 | //Things to write to serial from Program memory. Saves 400 to 2k of RAM. 114 | FORCE_INLINE void serialprintPGM(const char *str) 115 | { 116 | char ch=pgm_read_byte(str); 117 | while(ch) 118 | { 119 | MYSERIAL.write(ch); 120 | ch=pgm_read_byte(++str); 121 | } 122 | } 123 | 124 | 125 | //Things to write to serial from Program memory. Saves 400 to 2k of RAM. 126 | FORCE_INLINE void ajgSerialprintPGM(const char *str) 127 | { 128 | char ch=pgm_read_byte(str); 129 | while(ch) 130 | { 131 | AjgSerial.write(ch); 132 | ch=pgm_read_byte(++str); 133 | } 134 | } 135 | 136 | 137 | 138 | void get_command(); 139 | void process_commands(); 140 | 141 | void manage_inactivity(); 142 | 143 | #if defined(DUAL_X_CARRIAGE) && defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1 \ 144 | && defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1 145 | #define enable_x() do { WRITE(X_ENABLE_PIN, X_ENABLE_ON); WRITE(X2_ENABLE_PIN, X_ENABLE_ON); } while (0) 146 | #define disable_x() do { WRITE(X_ENABLE_PIN,!X_ENABLE_ON); WRITE(X2_ENABLE_PIN,!X_ENABLE_ON); axis_known_position[X_AXIS] = false; } while (0) 147 | #elif defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1 148 | #define enable_x() WRITE(X_ENABLE_PIN, X_ENABLE_ON) 149 | #define disable_x() { WRITE(X_ENABLE_PIN,!X_ENABLE_ON); axis_known_position[X_AXIS] = false; } 150 | #else 151 | #define enable_x() ; 152 | #define disable_x() ; 153 | #endif 154 | 155 | #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1 156 | #ifdef Y_DUAL_STEPPER_DRIVERS 157 | #define enable_y() { WRITE(Y_ENABLE_PIN, Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN, Y_ENABLE_ON); } 158 | #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN, !Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; } 159 | #else 160 | #define enable_y() WRITE(Y_ENABLE_PIN, Y_ENABLE_ON) 161 | #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; } 162 | #endif 163 | #else 164 | #define enable_y() ; 165 | #define disable_y() ; 166 | #endif 167 | 168 | #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1 169 | #ifdef Z_DUAL_STEPPER_DRIVERS 170 | #define enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON);} 171 | #define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; } 172 | #else 173 | #define enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON) 174 | #define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; } 175 | #endif 176 | #else 177 | #define enable_z() ; 178 | #define disable_z() ; 179 | #endif 180 | 181 | #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1) 182 | #define enable_e0() WRITE(E0_ENABLE_PIN, E_ENABLE_ON) 183 | #define disable_e0() WRITE(E0_ENABLE_PIN,!E_ENABLE_ON) 184 | #else 185 | #define enable_e0() /* nothing */ 186 | #define disable_e0() /* nothing */ 187 | #endif 188 | 189 | #if (EXTRUDERS > 1) && defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1) 190 | #define enable_e1() WRITE(E1_ENABLE_PIN, E_ENABLE_ON) 191 | #define disable_e1() WRITE(E1_ENABLE_PIN,!E_ENABLE_ON) 192 | #else 193 | #define enable_e1() /* nothing */ 194 | #define disable_e1() /* nothing */ 195 | #endif 196 | 197 | #if (EXTRUDERS > 2) && defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1) 198 | #define enable_e2() WRITE(E2_ENABLE_PIN, E_ENABLE_ON) 199 | #define disable_e2() WRITE(E2_ENABLE_PIN,!E_ENABLE_ON) 200 | #else 201 | #define enable_e2() /* nothing */ 202 | #define disable_e2() /* nothing */ 203 | #endif 204 | 205 | 206 | enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3}; 207 | 208 | 209 | void FlushSerialRequestResend(); 210 | void ClearToSend(); 211 | 212 | void FlushAjgSerialRequestResend(); 213 | void AjgClearToSend(); 214 | 215 | 216 | void get_coordinates(); 217 | #ifdef DELTA 218 | void calculate_delta(float cartesian[3]); 219 | extern float delta[3]; 220 | #endif 221 | #ifdef SCARA 222 | void calculate_delta(float cartesian[3]); 223 | void calculate_SCARA_forward_Transform(float f_scara[3]); 224 | #endif 225 | void prepare_move(); 226 | void kill(); 227 | void Stop(); 228 | 229 | bool IsStopped(); 230 | 231 | void enquecommand(const char *cmd); //put an ASCII command at the end of the current buffer. 232 | void enquecommand_P(const char *cmd); //put an ASCII command at the end of the current buffer, read from flash 233 | void prepare_arc_move(char isclockwise); 234 | void clamp_to_software_endstops(float target[3]); 235 | 236 | void refresh_cmd_timeout(void); 237 | 238 | #ifdef FAST_PWM_FAN 239 | void setPwmFrequency(uint8_t pin, int val); 240 | #endif 241 | 242 | #ifndef CRITICAL_SECTION_START 243 | #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli(); 244 | #define CRITICAL_SECTION_END SREG = _sreg; 245 | #endif //CRITICAL_SECTION_START 246 | 247 | extern float homing_feedrate[]; 248 | extern bool axis_relative_modes[]; 249 | extern int feedmultiply; 250 | extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders 251 | extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually 252 | extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner 253 | extern float current_position[NUM_AXIS] ; 254 | extern float add_homing[3]; 255 | #ifdef DELTA 256 | extern float endstop_adj[3]; 257 | extern float delta_radius; 258 | extern float delta_diagonal_rod; 259 | extern float delta_segments_per_second; 260 | void recalc_delta_settings(float radius, float diagonal_rod); 261 | #endif 262 | #ifdef SCARA 263 | extern float axis_scaling[3]; // Build size scaling 264 | #endif 265 | extern float min_pos[3]; 266 | extern float max_pos[3]; 267 | extern bool axis_known_position[3]; 268 | extern float zprobe_zoffset; 269 | extern int fanSpeed; 270 | #ifdef BARICUDA 271 | extern int ValvePressure; 272 | extern int EtoPPressure; 273 | #endif 274 | 275 | #ifdef FAN_SOFT_PWM 276 | extern unsigned char fanSpeedSoftPwm; 277 | #endif 278 | 279 | #ifdef FILAMENT_SENSOR 280 | extern float filament_width_nominal; //holds the theoretical filament diameter ie., 3.00 or 1.75 281 | extern bool filament_sensor; //indicates that filament sensor readings should control extrusion 282 | extern float filament_width_meas; //holds the filament diameter as accurately measured 283 | extern signed char measurement_delay[]; //ring buffer to delay measurement 284 | extern int delay_index1, delay_index2; //index into ring buffer 285 | extern float delay_dist; //delay distance counter 286 | extern int meas_delay_cm; //delay distance 287 | #endif 288 | 289 | #ifdef FWRETRACT 290 | extern bool autoretract_enabled; 291 | extern bool retracted[EXTRUDERS]; 292 | extern float retract_length, retract_length_swap, retract_feedrate, retract_zlift; 293 | extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate; 294 | #endif 295 | 296 | extern unsigned long starttime; 297 | extern unsigned long stoptime; 298 | 299 | // Handling multiple extruders pins 300 | extern uint8_t active_extruder; 301 | 302 | #ifdef DIGIPOT_I2C 303 | extern void digipot_i2c_set_current( int channel, float current ); 304 | extern void digipot_i2c_init(); 305 | #endif 306 | 307 | #endif 308 | -------------------------------------------------------------------------------- /speed_lookuptable.h: -------------------------------------------------------------------------------- 1 | #ifndef SPEED_LOOKUPTABLE_H 2 | #define SPEED_LOOKUPTABLE_H 3 | 4 | #include "Marlin.h" 5 | 6 | #if F_CPU == 16000000 7 | 8 | const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {\ 9 | { 62500, 55556}, { 6944, 3268}, { 3676, 1176}, { 2500, 607}, { 1893, 369}, { 1524, 249}, { 1275, 179}, { 1096, 135}, 10 | { 961, 105}, { 856, 85}, { 771, 69}, { 702, 58}, { 644, 49}, { 595, 42}, { 553, 37}, { 516, 32}, 11 | { 484, 28}, { 456, 25}, { 431, 23}, { 408, 20}, { 388, 19}, { 369, 16}, { 353, 16}, { 337, 14}, 12 | { 323, 13}, { 310, 11}, { 299, 11}, { 288, 11}, { 277, 9}, { 268, 9}, { 259, 8}, { 251, 8}, 13 | { 243, 8}, { 235, 7}, { 228, 6}, { 222, 6}, { 216, 6}, { 210, 6}, { 204, 5}, { 199, 5}, 14 | { 194, 5}, { 189, 4}, { 185, 4}, { 181, 4}, { 177, 4}, { 173, 4}, { 169, 4}, { 165, 3}, 15 | { 162, 3}, { 159, 4}, { 155, 3}, { 152, 3}, { 149, 2}, { 147, 3}, { 144, 3}, { 141, 2}, 16 | { 139, 3}, { 136, 2}, { 134, 2}, { 132, 3}, { 129, 2}, { 127, 2}, { 125, 2}, { 123, 2}, 17 | { 121, 2}, { 119, 1}, { 118, 2}, { 116, 2}, { 114, 1}, { 113, 2}, { 111, 2}, { 109, 1}, 18 | { 108, 2}, { 106, 1}, { 105, 2}, { 103, 1}, { 102, 1}, { 101, 1}, { 100, 2}, { 98, 1}, 19 | { 97, 1}, { 96, 1}, { 95, 2}, { 93, 1}, { 92, 1}, { 91, 1}, { 90, 1}, { 89, 1}, 20 | { 88, 1}, { 87, 1}, { 86, 1}, { 85, 1}, { 84, 1}, { 83, 0}, { 83, 1}, { 82, 1}, 21 | { 81, 1}, { 80, 1}, { 79, 1}, { 78, 0}, { 78, 1}, { 77, 1}, { 76, 1}, { 75, 0}, 22 | { 75, 1}, { 74, 1}, { 73, 1}, { 72, 0}, { 72, 1}, { 71, 1}, { 70, 0}, { 70, 1}, 23 | { 69, 0}, { 69, 1}, { 68, 1}, { 67, 0}, { 67, 1}, { 66, 0}, { 66, 1}, { 65, 0}, 24 | { 65, 1}, { 64, 1}, { 63, 0}, { 63, 1}, { 62, 0}, { 62, 1}, { 61, 0}, { 61, 1}, 25 | { 60, 0}, { 60, 0}, { 60, 1}, { 59, 0}, { 59, 1}, { 58, 0}, { 58, 1}, { 57, 0}, 26 | { 57, 1}, { 56, 0}, { 56, 0}, { 56, 1}, { 55, 0}, { 55, 1}, { 54, 0}, { 54, 0}, 27 | { 54, 1}, { 53, 0}, { 53, 0}, { 53, 1}, { 52, 0}, { 52, 0}, { 52, 1}, { 51, 0}, 28 | { 51, 0}, { 51, 1}, { 50, 0}, { 50, 0}, { 50, 1}, { 49, 0}, { 49, 0}, { 49, 1}, 29 | { 48, 0}, { 48, 0}, { 48, 1}, { 47, 0}, { 47, 0}, { 47, 0}, { 47, 1}, { 46, 0}, 30 | { 46, 0}, { 46, 1}, { 45, 0}, { 45, 0}, { 45, 0}, { 45, 1}, { 44, 0}, { 44, 0}, 31 | { 44, 0}, { 44, 1}, { 43, 0}, { 43, 0}, { 43, 0}, { 43, 1}, { 42, 0}, { 42, 0}, 32 | { 42, 0}, { 42, 1}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 1}, { 40, 0}, 33 | { 40, 0}, { 40, 0}, { 40, 0}, { 40, 1}, { 39, 0}, { 39, 0}, { 39, 0}, { 39, 0}, 34 | { 39, 1}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 1}, { 37, 0}, { 37, 0}, 35 | { 37, 0}, { 37, 0}, { 37, 0}, { 37, 1}, { 36, 0}, { 36, 0}, { 36, 0}, { 36, 0}, 36 | { 36, 1}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 1}, 37 | { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 1}, { 33, 0}, { 33, 0}, 38 | { 33, 0}, { 33, 0}, { 33, 0}, { 33, 0}, { 33, 1}, { 32, 0}, { 32, 0}, { 32, 0}, 39 | { 32, 0}, { 32, 0}, { 32, 0}, { 32, 0}, { 32, 1}, { 31, 0}, { 31, 0}, { 31, 0}, 40 | { 31, 0}, { 31, 0}, { 31, 0}, { 31, 1}, { 30, 0}, { 30, 0}, { 30, 0}, { 30, 0} 41 | }; 42 | 43 | const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {\ 44 | { 62500, 12500}, { 50000, 8334}, { 41666, 5952}, { 35714, 4464}, { 31250, 3473}, { 27777, 2777}, { 25000, 2273}, { 22727, 1894}, 45 | { 20833, 1603}, { 19230, 1373}, { 17857, 1191}, { 16666, 1041}, { 15625, 920}, { 14705, 817}, { 13888, 731}, { 13157, 657}, 46 | { 12500, 596}, { 11904, 541}, { 11363, 494}, { 10869, 453}, { 10416, 416}, { 10000, 385}, { 9615, 356}, { 9259, 331}, 47 | { 8928, 308}, { 8620, 287}, { 8333, 269}, { 8064, 252}, { 7812, 237}, { 7575, 223}, { 7352, 210}, { 7142, 198}, 48 | { 6944, 188}, { 6756, 178}, { 6578, 168}, { 6410, 160}, { 6250, 153}, { 6097, 145}, { 5952, 139}, { 5813, 132}, 49 | { 5681, 126}, { 5555, 121}, { 5434, 115}, { 5319, 111}, { 5208, 106}, { 5102, 102}, { 5000, 99}, { 4901, 94}, 50 | { 4807, 91}, { 4716, 87}, { 4629, 84}, { 4545, 81}, { 4464, 79}, { 4385, 75}, { 4310, 73}, { 4237, 71}, 51 | { 4166, 68}, { 4098, 66}, { 4032, 64}, { 3968, 62}, { 3906, 60}, { 3846, 59}, { 3787, 56}, { 3731, 55}, 52 | { 3676, 53}, { 3623, 52}, { 3571, 50}, { 3521, 49}, { 3472, 48}, { 3424, 46}, { 3378, 45}, { 3333, 44}, 53 | { 3289, 43}, { 3246, 41}, { 3205, 41}, { 3164, 39}, { 3125, 39}, { 3086, 38}, { 3048, 36}, { 3012, 36}, 54 | { 2976, 35}, { 2941, 35}, { 2906, 33}, { 2873, 33}, { 2840, 32}, { 2808, 31}, { 2777, 30}, { 2747, 30}, 55 | { 2717, 29}, { 2688, 29}, { 2659, 28}, { 2631, 27}, { 2604, 27}, { 2577, 26}, { 2551, 26}, { 2525, 25}, 56 | { 2500, 25}, { 2475, 25}, { 2450, 23}, { 2427, 24}, { 2403, 23}, { 2380, 22}, { 2358, 22}, { 2336, 22}, 57 | { 2314, 21}, { 2293, 21}, { 2272, 20}, { 2252, 20}, { 2232, 20}, { 2212, 20}, { 2192, 19}, { 2173, 18}, 58 | { 2155, 19}, { 2136, 18}, { 2118, 18}, { 2100, 17}, { 2083, 17}, { 2066, 17}, { 2049, 17}, { 2032, 16}, 59 | { 2016, 16}, { 2000, 16}, { 1984, 16}, { 1968, 15}, { 1953, 16}, { 1937, 14}, { 1923, 15}, { 1908, 15}, 60 | { 1893, 14}, { 1879, 14}, { 1865, 14}, { 1851, 13}, { 1838, 14}, { 1824, 13}, { 1811, 13}, { 1798, 13}, 61 | { 1785, 12}, { 1773, 13}, { 1760, 12}, { 1748, 12}, { 1736, 12}, { 1724, 12}, { 1712, 12}, { 1700, 11}, 62 | { 1689, 12}, { 1677, 11}, { 1666, 11}, { 1655, 11}, { 1644, 11}, { 1633, 10}, { 1623, 11}, { 1612, 10}, 63 | { 1602, 10}, { 1592, 10}, { 1582, 10}, { 1572, 10}, { 1562, 10}, { 1552, 9}, { 1543, 10}, { 1533, 9}, 64 | { 1524, 9}, { 1515, 9}, { 1506, 9}, { 1497, 9}, { 1488, 9}, { 1479, 9}, { 1470, 9}, { 1461, 8}, 65 | { 1453, 8}, { 1445, 9}, { 1436, 8}, { 1428, 8}, { 1420, 8}, { 1412, 8}, { 1404, 8}, { 1396, 8}, 66 | { 1388, 7}, { 1381, 8}, { 1373, 7}, { 1366, 8}, { 1358, 7}, { 1351, 7}, { 1344, 8}, { 1336, 7}, 67 | { 1329, 7}, { 1322, 7}, { 1315, 7}, { 1308, 6}, { 1302, 7}, { 1295, 7}, { 1288, 6}, { 1282, 7}, 68 | { 1275, 6}, { 1269, 7}, { 1262, 6}, { 1256, 6}, { 1250, 7}, { 1243, 6}, { 1237, 6}, { 1231, 6}, 69 | { 1225, 6}, { 1219, 6}, { 1213, 6}, { 1207, 6}, { 1201, 5}, { 1196, 6}, { 1190, 6}, { 1184, 5}, 70 | { 1179, 6}, { 1173, 5}, { 1168, 6}, { 1162, 5}, { 1157, 5}, { 1152, 6}, { 1146, 5}, { 1141, 5}, 71 | { 1136, 5}, { 1131, 5}, { 1126, 5}, { 1121, 5}, { 1116, 5}, { 1111, 5}, { 1106, 5}, { 1101, 5}, 72 | { 1096, 5}, { 1091, 5}, { 1086, 4}, { 1082, 5}, { 1077, 5}, { 1072, 4}, { 1068, 5}, { 1063, 4}, 73 | { 1059, 5}, { 1054, 4}, { 1050, 4}, { 1046, 5}, { 1041, 4}, { 1037, 4}, { 1033, 5}, { 1028, 4}, 74 | { 1024, 4}, { 1020, 4}, { 1016, 4}, { 1012, 4}, { 1008, 4}, { 1004, 4}, { 1000, 4}, { 996, 4}, 75 | { 992, 4}, { 988, 4}, { 984, 4}, { 980, 4}, { 976, 4}, { 972, 4}, { 968, 3}, { 965, 3} 76 | }; 77 | 78 | #elif F_CPU == 20000000 79 | 80 | const uint16_t speed_lookuptable_fast[256][2] PROGMEM = { 81 | {62500, 54055}, {8445, 3917}, {4528, 1434}, {3094, 745}, {2349, 456}, {1893, 307}, {1586, 222}, {1364, 167}, 82 | {1197, 131}, {1066, 105}, {961, 86}, {875, 72}, {803, 61}, {742, 53}, {689, 45}, {644, 40}, 83 | {604, 35}, {569, 32}, {537, 28}, {509, 25}, {484, 23}, {461, 21}, {440, 19}, {421, 17}, 84 | {404, 16}, {388, 15}, {373, 14}, {359, 13}, {346, 12}, {334, 11}, {323, 10}, {313, 10}, 85 | {303, 9}, {294, 9}, {285, 8}, {277, 7}, {270, 8}, {262, 7}, {255, 6}, {249, 6}, 86 | {243, 6}, {237, 6}, {231, 5}, {226, 5}, {221, 5}, {216, 5}, {211, 4}, {207, 5}, 87 | {202, 4}, {198, 4}, {194, 4}, {190, 3}, {187, 4}, {183, 3}, {180, 3}, {177, 4}, 88 | {173, 3}, {170, 3}, {167, 2}, {165, 3}, {162, 3}, {159, 2}, {157, 3}, {154, 2}, 89 | {152, 3}, {149, 2}, {147, 2}, {145, 2}, {143, 2}, {141, 2}, {139, 2}, {137, 2}, 90 | {135, 2}, {133, 2}, {131, 2}, {129, 1}, {128, 2}, {126, 2}, {124, 1}, {123, 2}, 91 | {121, 1}, {120, 2}, {118, 1}, {117, 1}, {116, 2}, {114, 1}, {113, 1}, {112, 2}, 92 | {110, 1}, {109, 1}, {108, 1}, {107, 2}, {105, 1}, {104, 1}, {103, 1}, {102, 1}, 93 | {101, 1}, {100, 1}, {99, 1}, {98, 1}, {97, 1}, {96, 1}, {95, 1}, {94, 1}, 94 | {93, 1}, {92, 1}, {91, 0}, {91, 1}, {90, 1}, {89, 1}, {88, 1}, {87, 0}, 95 | {87, 1}, {86, 1}, {85, 1}, {84, 0}, {84, 1}, {83, 1}, {82, 1}, {81, 0}, 96 | {81, 1}, {80, 1}, {79, 0}, {79, 1}, {78, 0}, {78, 1}, {77, 1}, {76, 0}, 97 | {76, 1}, {75, 0}, {75, 1}, {74, 1}, {73, 0}, {73, 1}, {72, 0}, {72, 1}, 98 | {71, 0}, {71, 1}, {70, 0}, {70, 1}, {69, 0}, {69, 1}, {68, 0}, {68, 1}, 99 | {67, 0}, {67, 1}, {66, 0}, {66, 1}, {65, 0}, {65, 0}, {65, 1}, {64, 0}, 100 | {64, 1}, {63, 0}, {63, 1}, {62, 0}, {62, 0}, {62, 1}, {61, 0}, {61, 1}, 101 | {60, 0}, {60, 0}, {60, 1}, {59, 0}, {59, 0}, {59, 1}, {58, 0}, {58, 0}, 102 | {58, 1}, {57, 0}, {57, 0}, {57, 1}, {56, 0}, {56, 0}, {56, 1}, {55, 0}, 103 | {55, 0}, {55, 1}, {54, 0}, {54, 0}, {54, 1}, {53, 0}, {53, 0}, {53, 0}, 104 | {53, 1}, {52, 0}, {52, 0}, {52, 1}, {51, 0}, {51, 0}, {51, 0}, {51, 1}, 105 | {50, 0}, {50, 0}, {50, 0}, {50, 1}, {49, 0}, {49, 0}, {49, 0}, {49, 1}, 106 | {48, 0}, {48, 0}, {48, 0}, {48, 1}, {47, 0}, {47, 0}, {47, 0}, {47, 1}, 107 | {46, 0}, {46, 0}, {46, 0}, {46, 0}, {46, 1}, {45, 0}, {45, 0}, {45, 0}, 108 | {45, 1}, {44, 0}, {44, 0}, {44, 0}, {44, 0}, {44, 1}, {43, 0}, {43, 0}, 109 | {43, 0}, {43, 0}, {43, 1}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, 110 | {42, 1}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 1}, {40, 0}, 111 | {40, 0}, {40, 0}, {40, 0}, {40, 1}, {39, 0}, {39, 0}, {39, 0}, {39, 0}, 112 | {39, 0}, {39, 0}, {39, 1}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, 113 | }; 114 | 115 | const uint16_t speed_lookuptable_slow[256][2] PROGMEM = { 116 | {62500, 10417}, {52083, 7441}, {44642, 5580}, {39062, 4340}, {34722, 3472}, {31250, 2841}, {28409, 2368}, {26041, 2003}, 117 | {24038, 1717}, {22321, 1488}, {20833, 1302}, {19531, 1149}, {18382, 1021}, {17361, 914}, {16447, 822}, {15625, 745}, 118 | {14880, 676}, {14204, 618}, {13586, 566}, {13020, 520}, {12500, 481}, {12019, 445}, {11574, 414}, {11160, 385}, 119 | {10775, 359}, {10416, 336}, {10080, 315}, {9765, 296}, {9469, 278}, {9191, 263}, {8928, 248}, {8680, 235}, 120 | {8445, 222}, {8223, 211}, {8012, 200}, {7812, 191}, {7621, 181}, {7440, 173}, {7267, 165}, {7102, 158}, 121 | {6944, 151}, {6793, 145}, {6648, 138}, {6510, 133}, {6377, 127}, {6250, 123}, {6127, 118}, {6009, 113}, 122 | {5896, 109}, {5787, 106}, {5681, 101}, {5580, 98}, {5482, 95}, {5387, 91}, {5296, 88}, {5208, 86}, 123 | {5122, 82}, {5040, 80}, {4960, 78}, {4882, 75}, {4807, 73}, {4734, 70}, {4664, 69}, {4595, 67}, 124 | {4528, 64}, {4464, 63}, {4401, 61}, {4340, 60}, {4280, 58}, {4222, 56}, {4166, 55}, {4111, 53}, 125 | {4058, 52}, {4006, 51}, {3955, 49}, {3906, 48}, {3858, 48}, {3810, 45}, {3765, 45}, {3720, 44}, 126 | {3676, 43}, {3633, 42}, {3591, 40}, {3551, 40}, {3511, 39}, {3472, 38}, {3434, 38}, {3396, 36}, 127 | {3360, 36}, {3324, 35}, {3289, 34}, {3255, 34}, {3221, 33}, {3188, 32}, {3156, 31}, {3125, 31}, 128 | {3094, 31}, {3063, 30}, {3033, 29}, {3004, 28}, {2976, 28}, {2948, 28}, {2920, 27}, {2893, 27}, 129 | {2866, 26}, {2840, 25}, {2815, 25}, {2790, 25}, {2765, 24}, {2741, 24}, {2717, 24}, {2693, 23}, 130 | {2670, 22}, {2648, 22}, {2626, 22}, {2604, 22}, {2582, 21}, {2561, 21}, {2540, 20}, {2520, 20}, 131 | {2500, 20}, {2480, 20}, {2460, 19}, {2441, 19}, {2422, 19}, {2403, 18}, {2385, 18}, {2367, 18}, 132 | {2349, 17}, {2332, 18}, {2314, 17}, {2297, 16}, {2281, 17}, {2264, 16}, {2248, 16}, {2232, 16}, 133 | {2216, 16}, {2200, 15}, {2185, 15}, {2170, 15}, {2155, 15}, {2140, 15}, {2125, 14}, {2111, 14}, 134 | {2097, 14}, {2083, 14}, {2069, 14}, {2055, 13}, {2042, 13}, {2029, 13}, {2016, 13}, {2003, 13}, 135 | {1990, 13}, {1977, 12}, {1965, 12}, {1953, 13}, {1940, 11}, {1929, 12}, {1917, 12}, {1905, 12}, 136 | {1893, 11}, {1882, 11}, {1871, 11}, {1860, 11}, {1849, 11}, {1838, 11}, {1827, 11}, {1816, 10}, 137 | {1806, 11}, {1795, 10}, {1785, 10}, {1775, 10}, {1765, 10}, {1755, 10}, {1745, 9}, {1736, 10}, 138 | {1726, 9}, {1717, 10}, {1707, 9}, {1698, 9}, {1689, 9}, {1680, 9}, {1671, 9}, {1662, 9}, 139 | {1653, 9}, {1644, 8}, {1636, 9}, {1627, 8}, {1619, 9}, {1610, 8}, {1602, 8}, {1594, 8}, 140 | {1586, 8}, {1578, 8}, {1570, 8}, {1562, 8}, {1554, 7}, {1547, 8}, {1539, 8}, {1531, 7}, 141 | {1524, 8}, {1516, 7}, {1509, 7}, {1502, 7}, {1495, 7}, {1488, 7}, {1481, 7}, {1474, 7}, 142 | {1467, 7}, {1460, 7}, {1453, 7}, {1446, 6}, {1440, 7}, {1433, 7}, {1426, 6}, {1420, 6}, 143 | {1414, 7}, {1407, 6}, {1401, 6}, {1395, 7}, {1388, 6}, {1382, 6}, {1376, 6}, {1370, 6}, 144 | {1364, 6}, {1358, 6}, {1352, 6}, {1346, 5}, {1341, 6}, {1335, 6}, {1329, 5}, {1324, 6}, 145 | {1318, 5}, {1313, 6}, {1307, 5}, {1302, 6}, {1296, 5}, {1291, 5}, {1286, 6}, {1280, 5}, 146 | {1275, 5}, {1270, 5}, {1265, 5}, {1260, 5}, {1255, 5}, {1250, 5}, {1245, 5}, {1240, 5}, 147 | {1235, 5}, {1230, 5}, {1225, 5}, {1220, 5}, {1215, 4}, {1211, 5}, {1206, 5}, {1201, 5}, 148 | }; 149 | 150 | #endif 151 | 152 | #endif 153 | -------------------------------------------------------------------------------- /LiquidCrystalRus.cpp: -------------------------------------------------------------------------------- 1 | #include "LiquidCrystalRus.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined(ARDUINO) && ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | // it is a Russian alphabet translation 15 | // except 0401 --> 0xa2 = ╗, 0451 --> 0xb5 16 | const PROGMEM uint8_t utf_recode[] = 17 | { 0x41,0xa0,0x42,0xa1,0xe0,0x45,0xa3,0xa4,0xa5,0xa6,0x4b,0xa7,0x4d,0x48,0x4f, 18 | 0xa8,0x50,0x43,0x54,0xa9,0xaa,0x58,0xe1,0xab,0xac,0xe2,0xad,0xae,0x62,0xaf,0xb0,0xb1, 19 | 0x61,0xb2,0xb3,0xb4,0xe3,0x65,0xb6,0xb7,0xb8,0xb9,0xba,0xbb,0xbc,0xbd,0x6f, 20 | 0xbe,0x70,0x63,0xbf,0x79,0xe4,0x78,0xe5,0xc0,0xc1,0xe6,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7 21 | }; 22 | 23 | // When the display powers up, it is configured as follows: 24 | // 25 | // 1. Display clear 26 | // 2. Function set: 27 | // DL = 1; 8-bit interface data 28 | // N = 0; 1-line display 29 | // F = 0; 5x8 dot character font 30 | // 3. Display on/off control: 31 | // D = 0; Display off 32 | // C = 0; Cursor off 33 | // B = 0; Blinking off 34 | // 4. Entry mode set: 35 | // I/D = 1; Increment by 1 36 | // S = 0; No shift 37 | // 38 | // Note, however, that resetting the Arduino doesn't reset the LCD, so we 39 | // can't assume that it's in that state when a sketch starts (and the 40 | // LiquidCrystal constructor is called). 41 | // 42 | // modified 27 Jul 2011 43 | // by Ilya V. Danilov http://mk90.ru/ 44 | 45 | 46 | LiquidCrystalRus::LiquidCrystalRus(uint8_t rs, uint8_t rw, uint8_t enable, 47 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 48 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) 49 | { 50 | init(0, rs, rw, enable, d0, d1, d2, d3, d4, d5, d6, d7); 51 | } 52 | 53 | LiquidCrystalRus::LiquidCrystalRus(uint8_t rs, uint8_t enable, 54 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 55 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) 56 | { 57 | init(0, rs, 255, enable, d0, d1, d2, d3, d4, d5, d6, d7); 58 | } 59 | 60 | LiquidCrystalRus::LiquidCrystalRus(uint8_t rs, uint8_t rw, uint8_t enable, 61 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3) 62 | { 63 | init(1, rs, rw, enable, d0, d1, d2, d3, 0, 0, 0, 0); 64 | } 65 | 66 | LiquidCrystalRus::LiquidCrystalRus(uint8_t rs, uint8_t enable, 67 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3) 68 | { 69 | init(1, rs, 255, enable, d0, d1, d2, d3, 0, 0, 0, 0); 70 | } 71 | 72 | void LiquidCrystalRus::init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable, 73 | uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, 74 | uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) 75 | { 76 | _rs_pin = rs; 77 | _rw_pin = rw; 78 | _enable_pin = enable; 79 | 80 | _data_pins[0] = d0; 81 | _data_pins[1] = d1; 82 | _data_pins[2] = d2; 83 | _data_pins[3] = d3; 84 | _data_pins[4] = d4; 85 | _data_pins[5] = d5; 86 | _data_pins[6] = d6; 87 | _data_pins[7] = d7; 88 | 89 | pinMode(_rs_pin, OUTPUT); 90 | // we can save 1 pin by not using RW. Indicate by passing 255 instead of pin# 91 | if (_rw_pin != 255) { 92 | pinMode(_rw_pin, OUTPUT); 93 | } 94 | pinMode(_enable_pin, OUTPUT); 95 | 96 | if (fourbitmode) 97 | _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS; 98 | else 99 | _displayfunction = LCD_8BITMODE | LCD_1LINE | LCD_5x8DOTS; 100 | 101 | begin(16, 1); 102 | } 103 | 104 | void LiquidCrystalRus::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) { 105 | if (lines > 1) { 106 | _displayfunction |= LCD_2LINE; 107 | } 108 | _numlines = lines; 109 | _currline = 0; 110 | 111 | // for some 1 line displays you can select a 10 pixel high font 112 | if ((dotsize != 0) && (lines == 1)) { 113 | _displayfunction |= LCD_5x10DOTS; 114 | } 115 | 116 | // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION! 117 | // according to datasheet, we need at least 40ms after power rises above 2.7V 118 | // before sending commands. Arduino can turn on way before 4.5V so we'll wait 50 119 | delayMicroseconds(50000); 120 | // Now we pull both RS and R/W low to begin commands 121 | digitalWrite(_rs_pin, LOW); 122 | digitalWrite(_enable_pin, LOW); 123 | if (_rw_pin != 255) { 124 | digitalWrite(_rw_pin, LOW); 125 | } 126 | 127 | //put the LCD into 4 bit or 8 bit mode 128 | if (! (_displayfunction & LCD_8BITMODE)) { 129 | // this is according to the Hitachi HD44780 datasheet 130 | // figure 24, pg 46 131 | 132 | // we start in 8bit mode, try to set 4 bit mode 133 | writeNbits(0x03,4); 134 | delayMicroseconds(4500); // wait min 4.1ms 135 | 136 | // second try 137 | writeNbits(0x03,4); 138 | delayMicroseconds(4500); // wait min 4.1ms 139 | 140 | // third go! 141 | writeNbits(0x03,4); 142 | delayMicroseconds(150); 143 | 144 | // finally, set to 8-bit interface 145 | writeNbits(0x02,4); 146 | } else { 147 | // this is according to the Hitachi HD44780 datasheet 148 | // page 45 figure 23 149 | 150 | // Send function set command sequence 151 | command(LCD_FUNCTIONSET | _displayfunction); 152 | delayMicroseconds(4500); // wait more than 4.1ms 153 | 154 | // second try 155 | command(LCD_FUNCTIONSET | _displayfunction); 156 | delayMicroseconds(150); 157 | 158 | // third go 159 | command(LCD_FUNCTIONSET | _displayfunction); 160 | } 161 | 162 | // finally, set # lines, font size, etc. 163 | command(LCD_FUNCTIONSET | _displayfunction); 164 | 165 | // turn the display on with no cursor or blinking default 166 | _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF; 167 | display(); 168 | 169 | // clear it off 170 | clear(); 171 | 172 | // Initialize to default text direction (for romance languages) 173 | _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT; 174 | // set the entry mode 175 | command(LCD_ENTRYMODESET | _displaymode); 176 | 177 | } 178 | 179 | void LiquidCrystalRus::setDRAMModel(uint8_t model) { 180 | _dram_model = model; 181 | } 182 | 183 | /********** high level commands, for the user! */ 184 | void LiquidCrystalRus::clear() 185 | { 186 | command(LCD_CLEARDISPLAY); // clear display, set cursor position to zero 187 | delayMicroseconds(2000); // this command takes a long time! 188 | } 189 | 190 | void LiquidCrystalRus::home() 191 | { 192 | command(LCD_RETURNHOME); // set cursor position to zero 193 | delayMicroseconds(2000); // this command takes a long time! 194 | } 195 | 196 | void LiquidCrystalRus::setCursor(uint8_t col, uint8_t row) 197 | { 198 | int row_offsets[] = { 0x00, 0x40, 0x14, 0x54 }; 199 | if ( row >= _numlines ) { 200 | row = _numlines-1; // we count rows starting w/0 201 | } 202 | 203 | command(LCD_SETDDRAMADDR | (col + row_offsets[row])); 204 | } 205 | 206 | // Turn the display on/off (quickly) 207 | void LiquidCrystalRus::noDisplay() { 208 | _displaycontrol &= ~LCD_DISPLAYON; 209 | command(LCD_DISPLAYCONTROL | _displaycontrol); 210 | } 211 | void LiquidCrystalRus::display() { 212 | _displaycontrol |= LCD_DISPLAYON; 213 | command(LCD_DISPLAYCONTROL | _displaycontrol); 214 | } 215 | 216 | // Turns the underline cursor on/off 217 | void LiquidCrystalRus::noCursor() { 218 | _displaycontrol &= ~LCD_CURSORON; 219 | command(LCD_DISPLAYCONTROL | _displaycontrol); 220 | } 221 | void LiquidCrystalRus::cursor() { 222 | _displaycontrol |= LCD_CURSORON; 223 | command(LCD_DISPLAYCONTROL | _displaycontrol); 224 | } 225 | 226 | // Turn on and off the blinking cursor 227 | void LiquidCrystalRus::noBlink() { 228 | _displaycontrol &= ~LCD_BLINKON; 229 | command(LCD_DISPLAYCONTROL | _displaycontrol); 230 | } 231 | void LiquidCrystalRus::blink() { 232 | _displaycontrol |= LCD_BLINKON; 233 | command(LCD_DISPLAYCONTROL | _displaycontrol); 234 | } 235 | 236 | // These commands scroll the display without changing the RAM 237 | void LiquidCrystalRus::scrollDisplayLeft(void) { 238 | command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT); 239 | } 240 | void LiquidCrystalRus::scrollDisplayRight(void) { 241 | command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT); 242 | } 243 | 244 | // This is for text that flows Left to Right 245 | void LiquidCrystalRus::leftToRight(void) { 246 | _displaymode |= LCD_ENTRYLEFT; 247 | command(LCD_ENTRYMODESET | _displaymode); 248 | } 249 | 250 | // This is for text that flows Right to Left 251 | void LiquidCrystalRus::rightToLeft(void) { 252 | _displaymode &= ~LCD_ENTRYLEFT; 253 | command(LCD_ENTRYMODESET | _displaymode); 254 | } 255 | 256 | // This will 'right justify' text from the cursor 257 | void LiquidCrystalRus::autoscroll(void) { 258 | _displaymode |= LCD_ENTRYSHIFTINCREMENT; 259 | command(LCD_ENTRYMODESET | _displaymode); 260 | } 261 | 262 | // This will 'left justify' text from the cursor 263 | void LiquidCrystalRus::noAutoscroll(void) { 264 | _displaymode &= ~LCD_ENTRYSHIFTINCREMENT; 265 | command(LCD_ENTRYMODESET | _displaymode); 266 | } 267 | 268 | // Allows us to fill the first 8 CGRAM locations 269 | // with custom characters 270 | void LiquidCrystalRus::createChar(uint8_t location, uint8_t charmap[]) { 271 | location &= 0x7; // we only have 8 locations 0-7 272 | command(LCD_SETCGRAMADDR | (location << 3)); 273 | for (int i=0; i<8; i++) { 274 | write(charmap[i]); 275 | } 276 | } 277 | 278 | /*********** mid level commands, for sending data/cmds */ 279 | 280 | inline void LiquidCrystalRus::command(uint8_t value) { 281 | send(value, LOW); 282 | } 283 | 284 | #if defined(ARDUINO) && ARDUINO >= 100 285 | size_t LiquidCrystalRus::write(uint8_t value) 286 | #else 287 | void LiquidCrystalRus::write(uint8_t value) 288 | #endif 289 | { 290 | uint8_t out_char=value; 291 | 292 | if (_dram_model == LCD_DRAM_WH1601) { 293 | uint8_t ac=recv(LOW) & 0x7f; 294 | if (ac>7 && ac<0x14) command(LCD_SETDDRAMADDR | (0x40+ac-8)); 295 | } 296 | 297 | if (value>=0x80) { // UTF-8 handling 298 | if (value >= 0xc0) { 299 | utf_hi_char = value - 0xd0; 300 | } else { 301 | value &= 0x3f; 302 | if (!utf_hi_char && (value == 1)) 303 | send(0xa2,HIGH); // ╗ 304 | else if ((utf_hi_char == 1) && (value == 0x11)) 305 | send(0xb5,HIGH); // ╦ 306 | else 307 | send(pgm_read_byte_near(utf_recode + value + (utf_hi_char<<6) - 0x10), HIGH); 308 | } 309 | } else send(out_char, HIGH); 310 | #if defined(ARDUINO) && ARDUINO >= 100 311 | return 1; // assume success 312 | #endif 313 | } 314 | 315 | /************ low level data pushing commands **********/ 316 | 317 | // write either command or data, with automatic 4/8-bit selection 318 | void LiquidCrystalRus::send(uint8_t value, uint8_t mode) { 319 | digitalWrite(_rs_pin, mode); 320 | 321 | // if there is a RW pin indicated, set it low to Write 322 | if (_rw_pin != 255) { 323 | digitalWrite(_rw_pin, LOW); 324 | } 325 | 326 | if (_displayfunction & LCD_8BITMODE) { 327 | writeNbits(value,8); 328 | } else { 329 | writeNbits(value>>4,4); 330 | writeNbits(value,4); 331 | } 332 | } 333 | 334 | // read data, with automatic 4/8-bit selection 335 | uint8_t LiquidCrystalRus::recv(uint8_t mode) { 336 | uint8_t retval; 337 | digitalWrite(_rs_pin, mode); 338 | 339 | // if there is a RW pin indicated, set it low to Write 340 | if (_rw_pin != 255) { 341 | digitalWrite(_rw_pin, HIGH); 342 | } 343 | 344 | if (_displayfunction & LCD_8BITMODE) { 345 | retval = readNbits(8); 346 | } else { 347 | retval = readNbits(4) << 4; 348 | retval |= readNbits(4); 349 | } 350 | return retval; 351 | } 352 | void LiquidCrystalRus::pulseEnable() { 353 | digitalWrite(_enable_pin, LOW); 354 | delayMicroseconds(1); 355 | digitalWrite(_enable_pin, HIGH); 356 | delayMicroseconds(1); // enable pulse must be >450ns 357 | digitalWrite(_enable_pin, LOW); 358 | delayMicroseconds(100); // commands need > 37us to settle 359 | } 360 | 361 | void LiquidCrystalRus::writeNbits(uint8_t value, uint8_t n) { 362 | for (int i = 0; i < n; i++) { 363 | pinMode(_data_pins[i], OUTPUT); 364 | digitalWrite(_data_pins[i], (value >> i) & 0x01); 365 | } 366 | 367 | pulseEnable(); 368 | } 369 | 370 | uint8_t LiquidCrystalRus::readNbits(uint8_t n) { 371 | uint8_t retval=0; 372 | for (int i = 0; i < n; i++) { 373 | pinMode(_data_pins[i], INPUT); 374 | } 375 | 376 | digitalWrite(_enable_pin, LOW); 377 | delayMicroseconds(1); 378 | digitalWrite(_enable_pin, HIGH); 379 | delayMicroseconds(1); // enable pulse must be >450ns 380 | 381 | for (int i = 0; i < n; i++) { 382 | retval |= (digitalRead(_data_pins[i]) == HIGH)?(1 << i):0; 383 | } 384 | 385 | digitalWrite(_enable_pin, LOW); 386 | 387 | return retval; 388 | } 389 | 390 | -------------------------------------------------------------------------------- /ConfigurationStore.cpp: -------------------------------------------------------------------------------- 1 | #include "Marlin.h" 2 | #include "planner.h" 3 | #include "temperature.h" 4 | #include "ultralcd.h" 5 | #include "ConfigurationStore.h" 6 | 7 | void _EEPROM_writeData(int &pos, uint8_t* value, uint8_t size) 8 | { 9 | do 10 | { 11 | eeprom_write_byte((unsigned char*)pos, *value); 12 | pos++; 13 | value++; 14 | }while(--size); 15 | } 16 | #define EEPROM_WRITE_VAR(pos, value) _EEPROM_writeData(pos, (uint8_t*)&value, sizeof(value)) 17 | void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) 18 | { 19 | do 20 | { 21 | *value = eeprom_read_byte((unsigned char*)pos); 22 | pos++; 23 | value++; 24 | }while(--size); 25 | } 26 | #define EEPROM_READ_VAR(pos, value) _EEPROM_readData(pos, (uint8_t*)&value, sizeof(value)) 27 | //====================================================================================== 28 | 29 | 30 | 31 | 32 | #define EEPROM_OFFSET 100 33 | 34 | 35 | // IMPORTANT: Whenever there are changes made to the variables stored in EEPROM 36 | // in the functions below, also increment the version number. This makes sure that 37 | // the default values are used whenever there is a change to the data, to prevent 38 | // wrong data being written to the variables. 39 | // ALSO: always make sure the variables in the Store and retrieve sections are in the same order. 40 | 41 | #define EEPROM_VERSION "V10" 42 | #ifdef DELTA 43 | #undef EEPROM_VERSION 44 | #define EEPROM_VERSION "V11" 45 | #endif 46 | #ifdef SCARA 47 | #undef EEPROM_VERSION 48 | #define EEPROM_VERSION "V12" 49 | #endif 50 | 51 | #ifdef EEPROM_SETTINGS 52 | void Config_StoreSettings() 53 | { 54 | char ver[4]= "000"; 55 | int i=EEPROM_OFFSET; 56 | EEPROM_WRITE_VAR(i,ver); // invalidate data first 57 | EEPROM_WRITE_VAR(i,axis_steps_per_unit); 58 | EEPROM_WRITE_VAR(i,max_feedrate); 59 | EEPROM_WRITE_VAR(i,max_acceleration_units_per_sq_second); 60 | EEPROM_WRITE_VAR(i,acceleration); 61 | EEPROM_WRITE_VAR(i,retract_acceleration); 62 | EEPROM_WRITE_VAR(i,minimumfeedrate); 63 | EEPROM_WRITE_VAR(i,mintravelfeedrate); 64 | EEPROM_WRITE_VAR(i,minsegmenttime); 65 | EEPROM_WRITE_VAR(i,max_xy_jerk); 66 | EEPROM_WRITE_VAR(i,max_z_jerk); 67 | EEPROM_WRITE_VAR(i,max_e_jerk); 68 | EEPROM_WRITE_VAR(i,add_homing); 69 | #ifdef DELTA 70 | EEPROM_WRITE_VAR(i,endstop_adj); 71 | EEPROM_WRITE_VAR(i,delta_radius); 72 | EEPROM_WRITE_VAR(i,delta_diagonal_rod); 73 | EEPROM_WRITE_VAR(i,delta_segments_per_second); 74 | #endif 75 | #ifndef ULTIPANEL 76 | int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED; 77 | int absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED; 78 | #endif 79 | EEPROM_WRITE_VAR(i,plaPreheatHotendTemp); 80 | EEPROM_WRITE_VAR(i,plaPreheatHPBTemp); 81 | EEPROM_WRITE_VAR(i,plaPreheatFanSpeed); 82 | EEPROM_WRITE_VAR(i,absPreheatHotendTemp); 83 | EEPROM_WRITE_VAR(i,absPreheatHPBTemp); 84 | EEPROM_WRITE_VAR(i,absPreheatFanSpeed); 85 | EEPROM_WRITE_VAR(i,zprobe_zoffset); 86 | #ifdef PIDTEMP 87 | EEPROM_WRITE_VAR(i,Kp); 88 | EEPROM_WRITE_VAR(i,Ki); 89 | EEPROM_WRITE_VAR(i,Kd); 90 | #else 91 | float dummy = 3000.0f; 92 | EEPROM_WRITE_VAR(i,dummy); 93 | dummy = 0.0f; 94 | EEPROM_WRITE_VAR(i,dummy); 95 | EEPROM_WRITE_VAR(i,dummy); 96 | #endif 97 | #ifndef DOGLCD 98 | int lcd_contrast = 32; 99 | #endif 100 | EEPROM_WRITE_VAR(i,lcd_contrast); 101 | #ifdef SCARA 102 | EEPROM_WRITE_VAR(i,axis_scaling); // Add scaling for SCARA 103 | #endif 104 | char ver2[4]=EEPROM_VERSION; 105 | i=EEPROM_OFFSET; 106 | EEPROM_WRITE_VAR(i,ver2); // validate data 107 | SERIAL_ECHO_START; 108 | SERIAL_ECHOLNPGM("Settings Stored"); 109 | } 110 | #endif //EEPROM_SETTINGS 111 | 112 | 113 | #ifndef DISABLE_M503 114 | void Config_PrintSettings() 115 | { // Always have this function, even with EEPROM_SETTINGS disabled, the current values will be shown 116 | SERIAL_ECHO_START; 117 | SERIAL_ECHOLNPGM("Steps per unit:"); 118 | SERIAL_ECHO_START; 119 | SERIAL_ECHOPAIR(" M92 X",axis_steps_per_unit[0]); 120 | SERIAL_ECHOPAIR(" Y",axis_steps_per_unit[1]); 121 | SERIAL_ECHOPAIR(" Z",axis_steps_per_unit[2]); 122 | SERIAL_ECHOPAIR(" E",axis_steps_per_unit[3]); 123 | SERIAL_ECHOLN(""); 124 | 125 | SERIAL_ECHO_START; 126 | #ifdef SCARA 127 | SERIAL_ECHOLNPGM("Scaling factors:"); 128 | SERIAL_ECHO_START; 129 | SERIAL_ECHOPAIR(" M365 X",axis_scaling[0]); 130 | SERIAL_ECHOPAIR(" Y",axis_scaling[1]); 131 | SERIAL_ECHOPAIR(" Z",axis_scaling[2]); 132 | SERIAL_ECHOLN(""); 133 | 134 | SERIAL_ECHO_START; 135 | #endif 136 | SERIAL_ECHOLNPGM("Maximum feedrates (mm/s):"); 137 | SERIAL_ECHO_START; 138 | SERIAL_ECHOPAIR(" M203 X",max_feedrate[0]); 139 | SERIAL_ECHOPAIR(" Y",max_feedrate[1] ); 140 | SERIAL_ECHOPAIR(" Z", max_feedrate[2] ); 141 | SERIAL_ECHOPAIR(" E", max_feedrate[3]); 142 | SERIAL_ECHOLN(""); 143 | 144 | SERIAL_ECHO_START; 145 | SERIAL_ECHOLNPGM("Maximum Acceleration (mm/s2):"); 146 | SERIAL_ECHO_START; 147 | SERIAL_ECHOPAIR(" M201 X" ,max_acceleration_units_per_sq_second[0] ); 148 | SERIAL_ECHOPAIR(" Y" , max_acceleration_units_per_sq_second[1] ); 149 | SERIAL_ECHOPAIR(" Z" ,max_acceleration_units_per_sq_second[2] ); 150 | SERIAL_ECHOPAIR(" E" ,max_acceleration_units_per_sq_second[3]); 151 | SERIAL_ECHOLN(""); 152 | SERIAL_ECHO_START; 153 | SERIAL_ECHOLNPGM("Acceleration: S=acceleration, T=retract acceleration"); 154 | SERIAL_ECHO_START; 155 | SERIAL_ECHOPAIR(" M204 S",acceleration ); 156 | SERIAL_ECHOPAIR(" T" ,retract_acceleration); 157 | SERIAL_ECHOLN(""); 158 | 159 | SERIAL_ECHO_START; 160 | SERIAL_ECHOLNPGM("Advanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s), Z=maximum Z jerk (mm/s), E=maximum E jerk (mm/s)"); 161 | SERIAL_ECHO_START; 162 | SERIAL_ECHOPAIR(" M205 S",minimumfeedrate ); 163 | SERIAL_ECHOPAIR(" T" ,mintravelfeedrate ); 164 | SERIAL_ECHOPAIR(" B" ,minsegmenttime ); 165 | SERIAL_ECHOPAIR(" X" ,max_xy_jerk ); 166 | SERIAL_ECHOPAIR(" Z" ,max_z_jerk); 167 | SERIAL_ECHOPAIR(" E" ,max_e_jerk); 168 | SERIAL_ECHOLN(""); 169 | 170 | SERIAL_ECHO_START; 171 | SERIAL_ECHOLNPGM("Home offset (mm):"); 172 | SERIAL_ECHO_START; 173 | SERIAL_ECHOPAIR(" M206 X",add_homing[0] ); 174 | SERIAL_ECHOPAIR(" Y" ,add_homing[1] ); 175 | SERIAL_ECHOPAIR(" Z" ,add_homing[2] ); 176 | SERIAL_ECHOLN(""); 177 | #ifdef DELTA 178 | SERIAL_ECHO_START; 179 | SERIAL_ECHOLNPGM("Endstop adjustement (mm):"); 180 | SERIAL_ECHO_START; 181 | SERIAL_ECHOPAIR(" M666 X",endstop_adj[0] ); 182 | SERIAL_ECHOPAIR(" Y" ,endstop_adj[1] ); 183 | SERIAL_ECHOPAIR(" Z" ,endstop_adj[2] ); 184 | SERIAL_ECHOLN(""); 185 | SERIAL_ECHO_START; 186 | SERIAL_ECHOLNPGM("Delta settings: L=delta_diagonal_rod, R=delta_radius, S=delta_segments_per_second"); 187 | SERIAL_ECHO_START; 188 | SERIAL_ECHOPAIR(" M665 L",delta_diagonal_rod ); 189 | SERIAL_ECHOPAIR(" R" ,delta_radius ); 190 | SERIAL_ECHOPAIR(" S" ,delta_segments_per_second ); 191 | SERIAL_ECHOLN(""); 192 | #endif 193 | #ifdef PIDTEMP 194 | SERIAL_ECHO_START; 195 | SERIAL_ECHOLNPGM("PID settings:"); 196 | SERIAL_ECHO_START; 197 | SERIAL_ECHOPAIR(" M301 P",Kp); 198 | SERIAL_ECHOPAIR(" I" ,unscalePID_i(Ki)); 199 | SERIAL_ECHOPAIR(" D" ,unscalePID_d(Kd)); 200 | SERIAL_ECHOLN(""); 201 | #endif 202 | } 203 | #endif 204 | 205 | 206 | #ifdef EEPROM_SETTINGS 207 | void Config_RetrieveSettings() 208 | { 209 | int i=EEPROM_OFFSET; 210 | char stored_ver[4]; 211 | char ver[4]=EEPROM_VERSION; 212 | EEPROM_READ_VAR(i,stored_ver); //read stored version 213 | // SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]"); 214 | if (strncmp(ver,stored_ver,3) == 0) 215 | { 216 | // version number match 217 | EEPROM_READ_VAR(i,axis_steps_per_unit); 218 | EEPROM_READ_VAR(i,max_feedrate); 219 | EEPROM_READ_VAR(i,max_acceleration_units_per_sq_second); 220 | 221 | // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner) 222 | reset_acceleration_rates(); 223 | 224 | EEPROM_READ_VAR(i,acceleration); 225 | EEPROM_READ_VAR(i,retract_acceleration); 226 | EEPROM_READ_VAR(i,minimumfeedrate); 227 | EEPROM_READ_VAR(i,mintravelfeedrate); 228 | EEPROM_READ_VAR(i,minsegmenttime); 229 | EEPROM_READ_VAR(i,max_xy_jerk); 230 | EEPROM_READ_VAR(i,max_z_jerk); 231 | EEPROM_READ_VAR(i,max_e_jerk); 232 | EEPROM_READ_VAR(i,add_homing); 233 | #ifdef DELTA 234 | EEPROM_READ_VAR(i,endstop_adj); 235 | EEPROM_READ_VAR(i,delta_radius); 236 | EEPROM_READ_VAR(i,delta_diagonal_rod); 237 | EEPROM_READ_VAR(i,delta_segments_per_second); 238 | #endif 239 | #ifndef ULTIPANEL 240 | int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed; 241 | int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed; 242 | #endif 243 | EEPROM_READ_VAR(i,plaPreheatHotendTemp); 244 | EEPROM_READ_VAR(i,plaPreheatHPBTemp); 245 | EEPROM_READ_VAR(i,plaPreheatFanSpeed); 246 | EEPROM_READ_VAR(i,absPreheatHotendTemp); 247 | EEPROM_READ_VAR(i,absPreheatHPBTemp); 248 | EEPROM_READ_VAR(i,absPreheatFanSpeed); 249 | EEPROM_READ_VAR(i,zprobe_zoffset); 250 | #ifndef PIDTEMP 251 | float Kp,Ki,Kd; 252 | #endif 253 | // do not need to scale PID values as the values in EEPROM are already scaled 254 | EEPROM_READ_VAR(i,Kp); 255 | EEPROM_READ_VAR(i,Ki); 256 | EEPROM_READ_VAR(i,Kd); 257 | #ifndef DOGLCD 258 | int lcd_contrast; 259 | #endif 260 | EEPROM_READ_VAR(i,lcd_contrast); 261 | #ifdef SCARA 262 | EEPROM_READ_VAR(i,axis_scaling); 263 | #endif 264 | 265 | // Call updatePID (similar to when we have processed M301) 266 | updatePID(); 267 | SERIAL_ECHO_START; 268 | SERIAL_ECHOLNPGM("Stored settings retrieved"); 269 | } 270 | else 271 | { 272 | Config_ResetDefault(); 273 | } 274 | #ifdef EEPROM_CHITCHAT 275 | Config_PrintSettings(); 276 | #endif 277 | } 278 | #endif 279 | 280 | void Config_ResetDefault() 281 | { 282 | float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT; 283 | float tmp2[]=DEFAULT_MAX_FEEDRATE; 284 | long tmp3[]=DEFAULT_MAX_ACCELERATION; 285 | for (short i=0;i<4;i++) 286 | { 287 | axis_steps_per_unit[i]=tmp1[i]; 288 | max_feedrate[i]=tmp2[i]; 289 | max_acceleration_units_per_sq_second[i]=tmp3[i]; 290 | #ifdef SCARA 291 | axis_scaling[i]=1; 292 | #endif 293 | } 294 | 295 | // steps per sq second need to be updated to agree with the units per sq second 296 | reset_acceleration_rates(); 297 | 298 | acceleration=DEFAULT_ACCELERATION; 299 | retract_acceleration=DEFAULT_RETRACT_ACCELERATION; 300 | minimumfeedrate=DEFAULT_MINIMUMFEEDRATE; 301 | minsegmenttime=DEFAULT_MINSEGMENTTIME; 302 | mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE; 303 | max_xy_jerk=DEFAULT_XYJERK; 304 | max_z_jerk=DEFAULT_ZJERK; 305 | max_e_jerk=DEFAULT_EJERK; 306 | add_homing[0] = add_homing[1] = add_homing[2] = 0; 307 | #ifdef DELTA 308 | endstop_adj[0] = endstop_adj[1] = endstop_adj[2] = 0; 309 | delta_radius= DELTA_RADIUS; 310 | delta_diagonal_rod= DELTA_DIAGONAL_ROD; 311 | delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND; 312 | recalc_delta_settings(delta_radius, delta_diagonal_rod); 313 | #endif 314 | #ifdef ULTIPANEL 315 | plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP; 316 | plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP; 317 | plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED; 318 | absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP; 319 | absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP; 320 | absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED; 321 | #endif 322 | #ifdef ENABLE_AUTO_BED_LEVELING 323 | zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER; 324 | #endif 325 | #ifdef DOGLCD 326 | lcd_contrast = DEFAULT_LCD_CONTRAST; 327 | #endif 328 | #ifdef PIDTEMP 329 | Kp = DEFAULT_Kp; 330 | Ki = scalePID_i(DEFAULT_Ki); 331 | Kd = scalePID_d(DEFAULT_Kd); 332 | 333 | // call updatePID (similar to when we have processed M301) 334 | updatePID(); 335 | 336 | #ifdef PID_ADD_EXTRUSION_RATE 337 | Kc = DEFAULT_Kc; 338 | #endif//PID_ADD_EXTRUSION_RATE 339 | #endif//PIDTEMP 340 | 341 | SERIAL_ECHO_START; 342 | SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded"); 343 | 344 | } 345 | -------------------------------------------------------------------------------- /SdVolume.cpp: -------------------------------------------------------------------------------- 1 | /* Arduino SdFat Library 2 | * Copyright (C) 2009 by William Greiman 3 | * 4 | * This file is part of the Arduino SdFat Library 5 | * 6 | * This Library is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This Library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with the Arduino SdFat Library. If not, see 18 | * . 19 | */ 20 | #include "Marlin.h" 21 | #ifdef SDSUPPORT 22 | 23 | #include "SdVolume.h" 24 | //------------------------------------------------------------------------------ 25 | #if !USE_MULTIPLE_CARDS 26 | // raw block cache 27 | uint32_t SdVolume::cacheBlockNumber_; // current block number 28 | cache_t SdVolume::cacheBuffer_; // 512 byte cache for Sd2Card 29 | Sd2Card* SdVolume::sdCard_; // pointer to SD card object 30 | bool SdVolume::cacheDirty_; // cacheFlush() will write block if true 31 | uint32_t SdVolume::cacheMirrorBlock_; // mirror block for second FAT 32 | #endif // USE_MULTIPLE_CARDS 33 | //------------------------------------------------------------------------------ 34 | // find a contiguous group of clusters 35 | bool SdVolume::allocContiguous(uint32_t count, uint32_t* curCluster) { 36 | // start of group 37 | uint32_t bgnCluster; 38 | // end of group 39 | uint32_t endCluster; 40 | // last cluster of FAT 41 | uint32_t fatEnd = clusterCount_ + 1; 42 | 43 | // flag to save place to start next search 44 | bool setStart; 45 | 46 | // set search start cluster 47 | if (*curCluster) { 48 | // try to make file contiguous 49 | bgnCluster = *curCluster + 1; 50 | 51 | // don't save new start location 52 | setStart = false; 53 | } else { 54 | // start at likely place for free cluster 55 | bgnCluster = allocSearchStart_; 56 | 57 | // save next search start if one cluster 58 | setStart = count == 1; 59 | } 60 | // end of group 61 | endCluster = bgnCluster; 62 | 63 | // search the FAT for free clusters 64 | for (uint32_t n = 0;; n++, endCluster++) { 65 | // can't find space checked all clusters 66 | if (n >= clusterCount_) goto fail; 67 | 68 | // past end - start from beginning of FAT 69 | if (endCluster > fatEnd) { 70 | bgnCluster = endCluster = 2; 71 | } 72 | uint32_t f; 73 | if (!fatGet(endCluster, &f)) goto fail; 74 | 75 | if (f != 0) { 76 | // cluster in use try next cluster as bgnCluster 77 | bgnCluster = endCluster + 1; 78 | } else if ((endCluster - bgnCluster + 1) == count) { 79 | // done - found space 80 | break; 81 | } 82 | } 83 | // mark end of chain 84 | if (!fatPutEOC(endCluster)) goto fail; 85 | 86 | // link clusters 87 | while (endCluster > bgnCluster) { 88 | if (!fatPut(endCluster - 1, endCluster)) goto fail; 89 | endCluster--; 90 | } 91 | if (*curCluster != 0) { 92 | // connect chains 93 | if (!fatPut(*curCluster, bgnCluster)) goto fail; 94 | } 95 | // return first cluster number to caller 96 | *curCluster = bgnCluster; 97 | 98 | // remember possible next free cluster 99 | if (setStart) allocSearchStart_ = bgnCluster + 1; 100 | 101 | return true; 102 | 103 | fail: 104 | return false; 105 | } 106 | //------------------------------------------------------------------------------ 107 | bool SdVolume::cacheFlush() { 108 | if (cacheDirty_) { 109 | if (!sdCard_->writeBlock(cacheBlockNumber_, cacheBuffer_.data)) { 110 | goto fail; 111 | } 112 | // mirror FAT tables 113 | if (cacheMirrorBlock_) { 114 | if (!sdCard_->writeBlock(cacheMirrorBlock_, cacheBuffer_.data)) { 115 | goto fail; 116 | } 117 | cacheMirrorBlock_ = 0; 118 | } 119 | cacheDirty_ = 0; 120 | } 121 | return true; 122 | 123 | fail: 124 | return false; 125 | } 126 | //------------------------------------------------------------------------------ 127 | bool SdVolume::cacheRawBlock(uint32_t blockNumber, bool dirty) { 128 | if (cacheBlockNumber_ != blockNumber) { 129 | if (!cacheFlush()) goto fail; 130 | if (!sdCard_->readBlock(blockNumber, cacheBuffer_.data)) goto fail; 131 | cacheBlockNumber_ = blockNumber; 132 | } 133 | if (dirty) cacheDirty_ = true; 134 | return true; 135 | 136 | fail: 137 | return false; 138 | } 139 | //------------------------------------------------------------------------------ 140 | // return the size in bytes of a cluster chain 141 | bool SdVolume::chainSize(uint32_t cluster, uint32_t* size) { 142 | uint32_t s = 0; 143 | do { 144 | if (!fatGet(cluster, &cluster)) goto fail; 145 | s += 512UL << clusterSizeShift_; 146 | } while (!isEOC(cluster)); 147 | *size = s; 148 | return true; 149 | 150 | fail: 151 | return false; 152 | } 153 | //------------------------------------------------------------------------------ 154 | // Fetch a FAT entry 155 | bool SdVolume::fatGet(uint32_t cluster, uint32_t* value) { 156 | uint32_t lba; 157 | if (cluster > (clusterCount_ + 1)) goto fail; 158 | if (FAT12_SUPPORT && fatType_ == 12) { 159 | uint16_t index = cluster; 160 | index += index >> 1; 161 | lba = fatStartBlock_ + (index >> 9); 162 | if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail; 163 | index &= 0X1FF; 164 | uint16_t tmp = cacheBuffer_.data[index]; 165 | index++; 166 | if (index == 512) { 167 | if (!cacheRawBlock(lba + 1, CACHE_FOR_READ)) goto fail; 168 | index = 0; 169 | } 170 | tmp |= cacheBuffer_.data[index] << 8; 171 | *value = cluster & 1 ? tmp >> 4 : tmp & 0XFFF; 172 | return true; 173 | } 174 | if (fatType_ == 16) { 175 | lba = fatStartBlock_ + (cluster >> 8); 176 | } else if (fatType_ == 32) { 177 | lba = fatStartBlock_ + (cluster >> 7); 178 | } else { 179 | goto fail; 180 | } 181 | if (lba != cacheBlockNumber_) { 182 | if (!cacheRawBlock(lba, CACHE_FOR_READ)) goto fail; 183 | } 184 | if (fatType_ == 16) { 185 | *value = cacheBuffer_.fat16[cluster & 0XFF]; 186 | } else { 187 | *value = cacheBuffer_.fat32[cluster & 0X7F] & FAT32MASK; 188 | } 189 | return true; 190 | 191 | fail: 192 | return false; 193 | } 194 | //------------------------------------------------------------------------------ 195 | // Store a FAT entry 196 | bool SdVolume::fatPut(uint32_t cluster, uint32_t value) { 197 | uint32_t lba; 198 | // error if reserved cluster 199 | if (cluster < 2) goto fail; 200 | 201 | // error if not in FAT 202 | if (cluster > (clusterCount_ + 1)) goto fail; 203 | 204 | if (FAT12_SUPPORT && fatType_ == 12) { 205 | uint16_t index = cluster; 206 | index += index >> 1; 207 | lba = fatStartBlock_ + (index >> 9); 208 | if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail; 209 | // mirror second FAT 210 | if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_; 211 | index &= 0X1FF; 212 | uint8_t tmp = value; 213 | if (cluster & 1) { 214 | tmp = (cacheBuffer_.data[index] & 0XF) | tmp << 4; 215 | } 216 | cacheBuffer_.data[index] = tmp; 217 | index++; 218 | if (index == 512) { 219 | lba++; 220 | index = 0; 221 | if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail; 222 | // mirror second FAT 223 | if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_; 224 | } 225 | tmp = value >> 4; 226 | if (!(cluster & 1)) { 227 | tmp = ((cacheBuffer_.data[index] & 0XF0)) | tmp >> 4; 228 | } 229 | cacheBuffer_.data[index] = tmp; 230 | return true; 231 | } 232 | if (fatType_ == 16) { 233 | lba = fatStartBlock_ + (cluster >> 8); 234 | } else if (fatType_ == 32) { 235 | lba = fatStartBlock_ + (cluster >> 7); 236 | } else { 237 | goto fail; 238 | } 239 | if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) goto fail; 240 | // store entry 241 | if (fatType_ == 16) { 242 | cacheBuffer_.fat16[cluster & 0XFF] = value; 243 | } else { 244 | cacheBuffer_.fat32[cluster & 0X7F] = value; 245 | } 246 | // mirror second FAT 247 | if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_; 248 | return true; 249 | 250 | fail: 251 | return false; 252 | } 253 | //------------------------------------------------------------------------------ 254 | // free a cluster chain 255 | bool SdVolume::freeChain(uint32_t cluster) { 256 | uint32_t next; 257 | 258 | // clear free cluster location 259 | allocSearchStart_ = 2; 260 | 261 | do { 262 | if (!fatGet(cluster, &next)) goto fail; 263 | 264 | // free cluster 265 | if (!fatPut(cluster, 0)) goto fail; 266 | 267 | cluster = next; 268 | } while (!isEOC(cluster)); 269 | 270 | return true; 271 | 272 | fail: 273 | return false; 274 | } 275 | //------------------------------------------------------------------------------ 276 | /** Volume free space in clusters. 277 | * 278 | * \return Count of free clusters for success or -1 if an error occurs. 279 | */ 280 | int32_t SdVolume::freeClusterCount() { 281 | uint32_t free = 0; 282 | uint16_t n; 283 | uint32_t todo = clusterCount_ + 2; 284 | 285 | if (fatType_ == 16) { 286 | n = 256; 287 | } else if (fatType_ == 32) { 288 | n = 128; 289 | } else { 290 | // put FAT12 here 291 | return -1; 292 | } 293 | 294 | for (uint32_t lba = fatStartBlock_; todo; todo -= n, lba++) { 295 | if (!cacheRawBlock(lba, CACHE_FOR_READ)) return -1; 296 | if (todo < n) n = todo; 297 | if (fatType_ == 16) { 298 | for (uint16_t i = 0; i < n; i++) { 299 | if (cacheBuffer_.fat16[i] == 0) free++; 300 | } 301 | } else { 302 | for (uint16_t i = 0; i < n; i++) { 303 | if (cacheBuffer_.fat32[i] == 0) free++; 304 | } 305 | } 306 | } 307 | return free; 308 | } 309 | //------------------------------------------------------------------------------ 310 | /** Initialize a FAT volume. 311 | * 312 | * \param[in] dev The SD card where the volume is located. 313 | * 314 | * \param[in] part The partition to be used. Legal values for \a part are 315 | * 1-4 to use the corresponding partition on a device formatted with 316 | * a MBR, Master Boot Record, or zero if the device is formatted as 317 | * a super floppy with the FAT boot sector in block zero. 318 | * 319 | * \return The value one, true, is returned for success and 320 | * the value zero, false, is returned for failure. Reasons for 321 | * failure include not finding a valid partition, not finding a valid 322 | * FAT file system in the specified partition or an I/O error. 323 | */ 324 | bool SdVolume::init(Sd2Card* dev, uint8_t part) { 325 | uint32_t totalBlocks; 326 | uint32_t volumeStartBlock = 0; 327 | fat32_boot_t* fbs; 328 | 329 | sdCard_ = dev; 330 | fatType_ = 0; 331 | allocSearchStart_ = 2; 332 | cacheDirty_ = 0; // cacheFlush() will write block if true 333 | cacheMirrorBlock_ = 0; 334 | cacheBlockNumber_ = 0XFFFFFFFF; 335 | 336 | // if part == 0 assume super floppy with FAT boot sector in block zero 337 | // if part > 0 assume mbr volume with partition table 338 | if (part) { 339 | if (part > 4)goto fail; 340 | if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail; 341 | part_t* p = &cacheBuffer_.mbr.part[part-1]; 342 | if ((p->boot & 0X7F) !=0 || 343 | p->totalSectors < 100 || 344 | p->firstSector == 0) { 345 | // not a valid partition 346 | goto fail; 347 | } 348 | volumeStartBlock = p->firstSector; 349 | } 350 | if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) goto fail; 351 | fbs = &cacheBuffer_.fbs32; 352 | if (fbs->bytesPerSector != 512 || 353 | fbs->fatCount == 0 || 354 | fbs->reservedSectorCount == 0 || 355 | fbs->sectorsPerCluster == 0) { 356 | // not valid FAT volume 357 | goto fail; 358 | } 359 | fatCount_ = fbs->fatCount; 360 | blocksPerCluster_ = fbs->sectorsPerCluster; 361 | // determine shift that is same as multiply by blocksPerCluster_ 362 | clusterSizeShift_ = 0; 363 | while (blocksPerCluster_ != (1 << clusterSizeShift_)) { 364 | // error if not power of 2 365 | if (clusterSizeShift_++ > 7) goto fail; 366 | } 367 | blocksPerFat_ = fbs->sectorsPerFat16 ? 368 | fbs->sectorsPerFat16 : fbs->sectorsPerFat32; 369 | 370 | fatStartBlock_ = volumeStartBlock + fbs->reservedSectorCount; 371 | 372 | // count for FAT16 zero for FAT32 373 | rootDirEntryCount_ = fbs->rootDirEntryCount; 374 | 375 | // directory start for FAT16 dataStart for FAT32 376 | rootDirStart_ = fatStartBlock_ + fbs->fatCount * blocksPerFat_; 377 | 378 | // data start for FAT16 and FAT32 379 | dataStartBlock_ = rootDirStart_ + ((32 * fbs->rootDirEntryCount + 511)/512); 380 | 381 | // total blocks for FAT16 or FAT32 382 | totalBlocks = fbs->totalSectors16 ? 383 | fbs->totalSectors16 : fbs->totalSectors32; 384 | // total data blocks 385 | clusterCount_ = totalBlocks - (dataStartBlock_ - volumeStartBlock); 386 | 387 | // divide by cluster size to get cluster count 388 | clusterCount_ >>= clusterSizeShift_; 389 | 390 | // FAT type is determined by cluster count 391 | if (clusterCount_ < 4085) { 392 | fatType_ = 12; 393 | if (!FAT12_SUPPORT) goto fail; 394 | } else if (clusterCount_ < 65525) { 395 | fatType_ = 16; 396 | } else { 397 | rootDirStart_ = fbs->fat32RootCluster; 398 | fatType_ = 32; 399 | } 400 | return true; 401 | 402 | fail: 403 | return false; 404 | } 405 | #endif 406 | -------------------------------------------------------------------------------- /Servo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | 22 | A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. 23 | The servos are pulsed in the background using the value most recently written using the write() method 24 | 25 | Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. 26 | Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. 27 | 28 | The methods are: 29 | 30 | Servo - Class for manipulating servo motors connected to Arduino pins. 31 | 32 | attach(pin ) - Attaches a servo motor to an i/o pin. 33 | attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds 34 | default min is 544, max is 2400 35 | 36 | write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) 37 | writeMicroseconds() - Sets the servo pulse width in microseconds 38 | read() - Gets the last written servo pulse width as an angle between 0 and 180. 39 | readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) 40 | attached() - Returns true if there is a servo attached. 41 | detach() - Stops an attached servos from pulsing its i/o pin. 42 | 43 | */ 44 | #include "Configuration.h" 45 | 46 | #ifdef NUM_SERVOS 47 | #include 48 | #include 49 | 50 | #include "Servo.h" 51 | 52 | #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 53 | #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds 54 | 55 | 56 | #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 57 | 58 | //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) 59 | 60 | static servo_t servos[MAX_SERVOS]; // static array of servo structures 61 | static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) 62 | 63 | uint8_t ServoCount = 0; // the total number of attached servos 64 | 65 | 66 | // convenience macros 67 | #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo 68 | #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer 69 | #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel 70 | #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel 71 | 72 | #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo 73 | #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo 74 | 75 | /************ static functions common to all instances ***********************/ 76 | 77 | static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) 78 | { 79 | if( Channel[timer] < 0 ) 80 | *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 81 | else{ 82 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) 83 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated 84 | } 85 | 86 | Channel[timer]++; // increment to the next channel 87 | if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { 88 | *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; 89 | if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated 90 | digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high 91 | } 92 | else { 93 | // finished all channels so wait for the refresh period to expire before starting over 94 | if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed 95 | *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); 96 | else 97 | *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed 98 | Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel 99 | } 100 | } 101 | 102 | #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform 103 | // Interrupt handlers for Arduino 104 | #if defined(_useTimer1) 105 | SIGNAL (TIMER1_COMPA_vect) 106 | { 107 | handle_interrupts(_timer1, &TCNT1, &OCR1A); 108 | } 109 | #endif 110 | 111 | #if defined(_useTimer3) 112 | SIGNAL (TIMER3_COMPA_vect) 113 | { 114 | handle_interrupts(_timer3, &TCNT3, &OCR3A); 115 | } 116 | #endif 117 | 118 | #if defined(_useTimer4) 119 | SIGNAL (TIMER4_COMPA_vect) 120 | { 121 | handle_interrupts(_timer4, &TCNT4, &OCR4A); 122 | } 123 | #endif 124 | 125 | #if defined(_useTimer5) 126 | SIGNAL (TIMER5_COMPA_vect) 127 | { 128 | handle_interrupts(_timer5, &TCNT5, &OCR5A); 129 | } 130 | #endif 131 | 132 | #elif defined WIRING 133 | // Interrupt handlers for Wiring 134 | #if defined(_useTimer1) 135 | void Timer1Service() 136 | { 137 | handle_interrupts(_timer1, &TCNT1, &OCR1A); 138 | } 139 | #endif 140 | #if defined(_useTimer3) 141 | void Timer3Service() 142 | { 143 | handle_interrupts(_timer3, &TCNT3, &OCR3A); 144 | } 145 | #endif 146 | #endif 147 | 148 | 149 | static void initISR(timer16_Sequence_t timer) 150 | { 151 | #if defined (_useTimer1) 152 | if(timer == _timer1) { 153 | TCCR1A = 0; // normal counting mode 154 | TCCR1B = _BV(CS11); // set prescaler of 8 155 | TCNT1 = 0; // clear the timer count 156 | #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) 157 | TIFR |= _BV(OCF1A); // clear any pending interrupts; 158 | TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt 159 | #else 160 | // here if not ATmega8 or ATmega128 161 | TIFR1 |= _BV(OCF1A); // clear any pending interrupts; 162 | TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt 163 | #endif 164 | #if defined(WIRING) 165 | timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); 166 | #endif 167 | } 168 | #endif 169 | 170 | #if defined (_useTimer3) 171 | if(timer == _timer3) { 172 | TCCR3A = 0; // normal counting mode 173 | TCCR3B = _BV(CS31); // set prescaler of 8 174 | TCNT3 = 0; // clear the timer count 175 | #if defined(__AVR_ATmega128__) 176 | TIFR |= _BV(OCF3A); // clear any pending interrupts; 177 | ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt 178 | #else 179 | TIFR3 = _BV(OCF3A); // clear any pending interrupts; 180 | TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt 181 | #endif 182 | #if defined(WIRING) 183 | timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only 184 | #endif 185 | } 186 | #endif 187 | 188 | #if defined (_useTimer4) 189 | if(timer == _timer4) { 190 | TCCR4A = 0; // normal counting mode 191 | TCCR4B = _BV(CS41); // set prescaler of 8 192 | TCNT4 = 0; // clear the timer count 193 | TIFR4 = _BV(OCF4A); // clear any pending interrupts; 194 | TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt 195 | } 196 | #endif 197 | 198 | #if defined (_useTimer5) 199 | if(timer == _timer5) { 200 | TCCR5A = 0; // normal counting mode 201 | TCCR5B = _BV(CS51); // set prescaler of 8 202 | TCNT5 = 0; // clear the timer count 203 | TIFR5 = _BV(OCF5A); // clear any pending interrupts; 204 | TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt 205 | } 206 | #endif 207 | } 208 | 209 | static void finISR(timer16_Sequence_t timer) 210 | { 211 | //disable use of the given timer 212 | #if defined WIRING // Wiring 213 | if(timer == _timer1) { 214 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) 215 | TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt 216 | #else 217 | TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt 218 | #endif 219 | timerDetach(TIMER1OUTCOMPAREA_INT); 220 | } 221 | else if(timer == _timer3) { 222 | #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) 223 | TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt 224 | #else 225 | ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt 226 | #endif 227 | timerDetach(TIMER3OUTCOMPAREA_INT); 228 | } 229 | #else 230 | //For arduino - in future: call here to a currently undefined function to reset the timer 231 | #endif 232 | } 233 | 234 | static boolean isTimerActive(timer16_Sequence_t timer) 235 | { 236 | // returns true if any servo is active on this timer 237 | for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { 238 | if(SERVO(timer,channel).Pin.isActive == true) 239 | return true; 240 | } 241 | return false; 242 | } 243 | 244 | 245 | /****************** end of static functions ******************************/ 246 | 247 | Servo::Servo() 248 | { 249 | if( ServoCount < MAX_SERVOS) { 250 | this->servoIndex = ServoCount++; // assign a servo index to this instance 251 | servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 252 | } 253 | else 254 | this->servoIndex = INVALID_SERVO ; // too many servos 255 | } 256 | 257 | uint8_t Servo::attach(int pin) 258 | { 259 | return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); 260 | } 261 | 262 | uint8_t Servo::attach(int pin, int min, int max) 263 | { 264 | if(this->servoIndex < MAX_SERVOS ) { 265 | #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) 266 | if (pin > 0) this->pin = pin; else pin = this->pin; 267 | #endif 268 | pinMode( pin, OUTPUT) ; // set servo pin to output 269 | servos[this->servoIndex].Pin.nbr = pin; 270 | // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 271 | this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS 272 | this->max = (MAX_PULSE_WIDTH - max)/4; 273 | // initialize the timer if it has not already been initialized 274 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); 275 | if(isTimerActive(timer) == false) 276 | initISR(timer); 277 | servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive 278 | } 279 | return this->servoIndex ; 280 | } 281 | 282 | void Servo::detach() 283 | { 284 | servos[this->servoIndex].Pin.isActive = false; 285 | timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); 286 | if(isTimerActive(timer) == false) { 287 | finISR(timer); 288 | } 289 | } 290 | 291 | void Servo::write(int value) 292 | { 293 | if(value < MIN_PULSE_WIDTH) 294 | { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) 295 | if(value < 0) value = 0; 296 | if(value > 180) value = 180; 297 | value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); 298 | } 299 | this->writeMicroseconds(value); 300 | } 301 | 302 | void Servo::writeMicroseconds(int value) 303 | { 304 | // calculate and store the values for the given channel 305 | byte channel = this->servoIndex; 306 | if( (channel < MAX_SERVOS) ) // ensure channel is valid 307 | { 308 | if( value < SERVO_MIN() ) // ensure pulse width is valid 309 | value = SERVO_MIN(); 310 | else if( value > SERVO_MAX() ) 311 | value = SERVO_MAX(); 312 | 313 | value = value - TRIM_DURATION; 314 | value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009 315 | 316 | uint8_t oldSREG = SREG; 317 | cli(); 318 | servos[channel].ticks = value; 319 | SREG = oldSREG; 320 | } 321 | } 322 | 323 | int Servo::read() // return the value as degrees 324 | { 325 | return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); 326 | } 327 | 328 | int Servo::readMicroseconds() 329 | { 330 | unsigned int pulsewidth; 331 | if( this->servoIndex != INVALID_SERVO ) 332 | pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009 333 | else 334 | pulsewidth = 0; 335 | 336 | return pulsewidth; 337 | } 338 | 339 | bool Servo::attached() 340 | { 341 | return servos[this->servoIndex].Pin.isActive ; 342 | } 343 | 344 | #endif 345 | --------------------------------------------------------------------------------