diff options
author | erikdebruijn <erikdebruijn> | 2010-07-22 08:58:27 +0000 |
---|---|---|
committer | erikdebruijn <erikdebruijn@cb376a5e-1013-0410-a455-b6b1f9ac8223> | 2010-07-22 08:58:27 +0000 |
commit | 78c3a5991e0563142ab1765aec9d0fdd62a2a118 (patch) | |
tree | 9751cddb67c561953e6ac4202a63d81ead531186 | |
parent | 69cb8e18d065306d382d99833683f21a7c6a1360 (diff) | |
download | reprap-78c3a5991e0563142ab1765aec9d0fdd62a2a118.tar.gz reprap-78c3a5991e0563142ab1765aec9d0fdd62a2a118.zip |
My branch of te firmware that adds software PWM because of PWM conflicting with interrupts. This version allows you to run a RepRap on one Sanguino (or RepRap Motherboard) without an additional temperature controller.
- Heating is PID controlled. The algorithm is taken from the Makerbot firmware's main branch and integrated to work in this version. PID heating allows much better temperature stability and more reliable printing.
- This firmware is optimized to work with my 3D-to-5D-gcode.php script, also in my user dir in this SVN tree, but it should run most G-Code.
- Note: Reversing the extruder works, but is somewhat buggy.
- With a 16 MHz chrystal you can run 3 steppers at 1/16 microstepping (not four), so I'm running te Z at full stepping.
- The firmware allows you to use a potmeter to modify the amount that is extruded. This is an easy way to quickly configure this property on the fly. It provides feedback of its value on the serial port.
- My baud rate is different from the default, don't forget to adjust your PC side or this firmware.
git-svn-id: https://reprap.svn.sourceforge.net/svnroot/reprap@3658 cb376a5e-1013-0410-a455-b6b1f9ac8223
20 files changed, 3802 insertions, 0 deletions
diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde b/trunk/users/erik/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde new file mode 100644 index 00000000..acb13c56 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/FiveD_GCode_Interpreter.pde @@ -0,0 +1,363 @@ +// Yep, this is actually -*- c++ -*- + +// Sanguino G-code Interpreter +// Arduino v1.0 by Mike Ellery - initial software (mellery@gmail.com) +// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com) +// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com) +// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com) +// Sanguino v1.4 by Adrian Bowyer - added the Sanguino; extensive mods... (a.bowyer@bath.ac.uk) +// Sanguino v1.5 by Adrian Bowyer - implemented 4D Bressenham XYZ+ stepper control... (a.bowyer@bath.ac.uk) + +#include <ctype.h> +#include <HardwareSerial.h> +#include "WProgram.h" +#include "parameters.h" +#include "pins.h" +#include "extruder.h" +#include "vectors.h" +#include "cartesian_dda.h" + +// Inline interrupt control functions +inline void enableTimerInterrupt() +{ + TIMSK1 |= (1<<OCIE1A); +} + +inline void disableTimerInterrupt() +{ + TIMSK1 &= ~(1<<OCIE1A); +} + +inline void setTimerCeiling(unsigned int c) +{ + OCR1A = c; +} + +inline void resetTimer() +{ + TCNT2 = 0; +} +char debugstring[COMMAND_SIZE]; + +// Maintain a list of extruders... +byte extruder_in_use = 0; +extruder* ex[EXTRUDER_COUNT]; + +// ...creating static instances of them here +extruder ex0(EXTRUDER_0_MOTOR_DIR_PIN, EXTRUDER_0_MOTOR_SPEED_PIN , EXTRUDER_0_HEATER_PIN, + EXTRUDER_0_FAN_PIN, EXTRUDER_0_TEMPERATURE_PIN, EXTRUDER_0_VALVE_DIR_PIN, + EXTRUDER_0_VALVE_ENABLE_PIN, EXTRUDER_0_STEP_ENABLE_PIN); + +// Each entry in the buffer is an instance of cartesian_dda. + +cartesian_dda* cdda[BUFFER_SIZE]; + +cartesian_dda cdda0; +cartesian_dda cdda1; +cartesian_dda cdda2; +cartesian_dda cdda3; + +volatile byte head; +volatile byte tail; + +// Where the machine is from the point of view of the command stream + +FloatPoint where_i_am; + + +// Our interrupt function +SIGNAL(SIG_OUTPUT_COMPARE1A) +{ + disableTimerInterrupt(); + + if(cdda[tail]->active()) + cdda[tail]->dda_step(); + else + dQMove(); + + enableTimerInterrupt(); +} + +//ERIK +#ifdef MODIFIER_PIN +double modifier = 500.0; +double modifier_old = 1.0; +#endif +double modifier_speed = 512.0; //ERIK + +void setup() +{ + + disableTimerInterrupt(); + // This screws with PWM :( + setupTimerInterrupt(); + debugstring[0] = 0; + extruder_in_use = 0; + ex[extruder_in_use] = &ex0; + + head = 0; + tail = 0; + + cdda[0] = &cdda0; + cdda[1] = &cdda1; + cdda[2] = &cdda2; + cdda[3] = &cdda3; + + setExtruder(); + init_process_string(); + where_i_am.x = 0.0; + where_i_am.y = 0.0; + where_i_am.z = 0.0; + where_i_am.e = 0.0; + where_i_am.f = SLOW_XY_FEEDRATE; + + Serial.begin(57600); + Serial.println("start"); + setTimer(DEFAULT_TICK); + enableTimerInterrupt(); + + // ERIK: +#ifdef MODIFIER_PIN + pinMode(MODIFIER_PIN,INPUT); +#endif + +} + +byte softPWMduty = 0; +byte softPWMcount = 0; +int PWMstate = HIGH; +static void softPWM() +{ + if(softPWMduty == softPWMcount) + { + PWMstate = LOW; + } + digitalWrite(12,PWMstate); + + if(softPWMcount<254) + softPWMcount++; + else + { + PWMstate = HIGH; + softPWMcount=0; + } +} + + +void loop() +{ + manage_all_extruders(); + get_and_do_command(); +} + +//****************************************************************************************** + +// The move buffer + +inline bool qFull() +{ + if(tail == 0) + return head == (BUFFER_SIZE - 1); + else + return head == (tail - 1); +} + +inline bool qEmpty() +{ + return tail == head && !cdda[tail]->active(); +} + +inline void qMove(FloatPoint p) +{ + while(qFull()) delay(WAITING_DELAY); + byte h = head; + h++; + if(h >= BUFFER_SIZE) + h = 0; + cdda[h]->set_target(p); + head = h; +} + +inline void dQMove() +{ + if(qEmpty()) + return; + byte t = tail; + t++; + if(t >= BUFFER_SIZE) + t = 0; + cdda[t]->dda_start(); + tail = t; +} + +inline void setUnits(bool u) +{ + for(byte i = 0; i < BUFFER_SIZE; i++) + cdda[i]->set_units(u); +} + +inline void setExtruder() +{ + for(byte i = 0; i < BUFFER_SIZE; i++) + cdda[i]->set_extruder(ex[extruder_in_use]); +} + +inline void setPosition(FloatPoint p) +{ + where_i_am = p; +} + +//****************************************************************************************** + +// Interrupt functions +void setupTimerInterrupt() +{ + //clear the registers + TCCR1A = 0; + TCCR1B = 0; + TCCR1C = 0; + TIMSK1 = 0; + + //waveform generation = 0100 = CTC + TCCR1B &= ~(1<<WGM13); + TCCR1B |= (1<<WGM12); + TCCR1A &= ~(1<<WGM11); + TCCR1A &= ~(1<<WGM10); + + //output mode = 00 (disconnected) + TCCR1A &= ~(1<<COM1A1); + TCCR1A &= ~(1<<COM1A0); + TCCR1A &= ~(1<<COM1B1); + TCCR1A &= ~(1<<COM1B0); + + //start off with a slow frequency. + setTimerResolution(5); + setTimerCeiling(65535); +} + +void setTimerResolution(byte r) +{ + //here's how you figure out the tick size: + // 1000000 / ((16000000 / prescaler)) + // 1000000 = microseconds in 1 second + // 16000000 = cycles in 1 second + // prescaler = your prescaler + + // no prescaler == 0.0625 usec tick + if (r == 0) + { + // 001 = clk/1 + TCCR1B &= ~(1<<CS12); + TCCR1B &= ~(1<<CS11); + TCCR1B |= (1<<CS10); + } + // prescale of /8 == 0.5 usec tick + else if (r == 1) + { + // 010 = clk/8 + TCCR1B &= ~(1<<CS12); + TCCR1B |= (1<<CS11); + TCCR1B &= ~(1<<CS10); + } + // prescale of /64 == 4 usec tick + else if (r == 2) + { + // 011 = clk/64 + TCCR1B &= ~(1<<CS12); + TCCR1B |= (1<<CS11); + TCCR1B |= (1<<CS10); + } + // prescale of /256 == 16 usec tick + else if (r == 3) + { + // 100 = clk/256 + TCCR1B |= (1<<CS12); + TCCR1B &= ~(1<<CS11); + TCCR1B &= ~(1<<CS10); + } + // prescale of /1024 == 64 usec tick + else + { + // 101 = clk/1024 + TCCR1B |= (1<<CS12); + TCCR1B &= ~(1<<CS11); + TCCR1B |= (1<<CS10); + } +} + +unsigned int getTimerCeiling(long delay) +{ + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs) + if (delay <= 65535L) + return (delay & 0xffff); + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs) + else if (delay <= 524280L) + return ((delay / 8) & 0xffff); + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs) + else if (delay <= 4194240L) + return ((delay / 64) & 0xffff); + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs) + else if (delay <= 16776960L) + return ((delay / 256) & 0xffff); + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs) + else if (delay <= 67107840L) + return ((delay / 1024) & 0xffff); + //its really slow... hopefully we can just get by with super slow. + else + return 65535; +} + +byte getTimerResolution(long delay) +{ + // these also represent frequency: 1000000 / delay / 2 = frequency in hz. + + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs (4 millisecond max)) + // range: 8Mhz max - 122hz min + if (delay <= 65535L) + return 0; + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs (32 millisecond max)) + // range:1Mhz max - 15.26hz min + else if (delay <= 524280L) + return 1; + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs (0.26 seconds max)) + // range: 125Khz max - 1.9hz min + else if (delay <= 4194240L) + return 2; + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs (1.04 seconds max)) + // range: 31.25Khz max - 0.475hz min + else if (delay <= 16776960L) + return 3; + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max)) + // range: 7.812Khz max - 0.119hz min + else if (delay <= 67107840L) + return 4; + //its really slow... hopefully we can just get by with super slow. + else + return 4; +} + + +// Depending on how much work the interrupt function has to do, this is +// pretty accurate between 10 us and 0.1 s. At fast speeds, the time +// taken in the interrupt function becomes significant, of course. + +// Note - it is up to the user to call enableTimerInterrupt() after a call +// to this function. + +inline void setTimer(long delay) +{ + // delay is the delay between steps in microsecond ticks. + // + // we break it into 5 different resolutions based on the delay. + // then we set the resolution based on the size of the delay. + // we also then calculate the timer ceiling required. (ie what the counter counts to) + // the result is the timer counts up to the appropriate time and then fires an interrupt. + + // Actual ticks are 0.0625 us, so multiply delay by 16 + + delay <<= 4; + + setTimerCeiling(getTimerCeiling(delay)); + setTimerResolution(getTimerResolution(delay)); +} + diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/ThermistorTable.h b/trunk/users/erik/FiveD_GCode_Interpreter/ThermistorTable.h new file mode 100644 index 00000000..0d7ef948 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/ThermistorTable.h @@ -0,0 +1,48 @@ +#ifndef THERMISTORTABLE_H_ +#define THERMISTORTABLE_H_ + +// Uncomment the next line if you are using a thermistor; leave it if you have a thermocouple +//#define USE_THERMISTOR + + +// How accurately do we maintain the temperature? +#define HALF_DEAD_ZONE 5 + +// Thermistor lookup table for RepRap Temperature Sensor Boards (http://make.rrrf.org/ts) +// See this page: +// http://dev.www.reprap.org/bin/view/Main/Thermistor +// for details of what goes in this table. +// Made with createTemperatureLookup.py (http://svn.reprap.org/trunk/reprap/firmware/Arduino/utilities/createTemperatureLookup.py) +// ./createTemperatureLookup.py --r0=100000 --t0=25 --r1=0 --r2=4700 --beta=4066 --max-adc=1023 +// r0: 100000 +// t0: 25 +// r1: 0 +// r2: 4700 +// beta: 4066 +// max adc: 1023 +#ifdef USE_THERMISTOR +#define NUMTEMPS 20 +short temptable[NUMTEMPS][2] = { + {1, 841}, + {54, 255}, + {107, 209}, + {160, 184}, + {213, 166}, + {266, 153}, + {319, 142}, + {372, 132}, + {425, 124}, + {478, 116}, + {531, 108}, + {584, 101}, + {637, 93}, + {690, 86}, + {743, 78}, + {796, 70}, + {849, 61}, + {902, 50}, + {955, 34}, + {1008, 3} +}; +#endif +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h new file mode 100644 index 00000000..7ca3ed7c --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h @@ -0,0 +1,175 @@ +/* + * This class controls the movement of the RepRap machine. + * It implements a DDA in four dimensions, so the length of extruded + * filament is treated as a variable, just like X, Y, and Z. + * + * Adrian Bowyer 9 May 2009 + */ + +#ifndef CARTESIAN_DDA_H +#define CARTESIAN_DDA_H + +// Main class for moving the RepRap machine about + +class cartesian_dda +{ +private: + + extruder* ext; // The extruder I'm currently using - keep this up to date... + + FloatPoint units; // Factors for converting either mm or inches to steps + +// FloatPoint current_position; // Where the machine is + FloatPoint target_position; // Where it's going + FloatPoint delta_position; // The difference between the two + float distance; // How long the path is + + LongPoint current_steps; // Similar information as above in steps rather than units + LongPoint target_steps; + LongPoint delta_steps; + LongPoint dda_counter; // DDA error-accumulation variables + long t_scale; // When doing lots of t steps, scale them so the DDA doesn't spend for ever on them + + byte x_direction; // Am I going in the + or - direction? + byte y_direction; + byte z_direction; + byte e_direction; + byte f_direction; + + bool x_can_step; // Am I not at an endstop? Have I not reached the target? etc. + bool y_can_step; + bool z_can_step; + bool e_can_step; + bool f_can_step; + +// Variables for acceleration calculations + + long total_steps; // The number of steps to take along the longest movement axis + + long timestep; // microseconds + bool nullmove; // this move is zero length + bool real_move; // Flag to know if we've changed something physical + volatile bool live; // Flag for when we're plotting a line + +// Internal functions that need not concern the user + + // Take a single step + + void do_x_step(); + void do_y_step(); + void do_z_step(); + void do_e_step(); + + // Can this axis step? + + bool can_step(byte min_pin, byte max_pin, long current, long target, byte dir); + + // Read a limit switch + + bool read_switch(byte pin); + + // Work out the number of microseconds between steps + + long calculate_feedrate_delay(float feedrate); + + // Switch the steppers on and off + + void enable_steppers(); + void disable_steppers(); + + // Custom short delay function (microseconds) + + //void delayMicrosecondsInterruptible(unsigned int us); + + +public: + + cartesian_dda(); + + // Set where I'm going + + void set_target(const FloatPoint& p); + + // Start the DDA + + void dda_start(); + + // Do one step of the DDA + + void dda_step(); + + // Are we running at the moment? + + bool active(); + + // True for mm; false for inches + + void set_units(bool using_mm); + + // Record the selection of a new extruder + + void set_extruder(extruder* ex); +}; + +// Short functions inline to save memory; particularly useful in the Arduino + + +inline void cartesian_dda::set_extruder(extruder* ex) +{ + ext = ex; +} + +inline bool cartesian_dda::active() +{ + return live; +} + +inline void cartesian_dda::do_x_step() +{ + digitalWrite(X_STEP_PIN, LOW);// ERIK: Active LOW! + delayMicroseconds(5); + digitalWrite(X_STEP_PIN, HIGH); +} + +inline void cartesian_dda::do_y_step() +{ + digitalWrite(Y_STEP_PIN, LOW);// ERIK: Active LOW! + delayMicroseconds(5); + digitalWrite(Y_STEP_PIN, HIGH); +} + +inline void cartesian_dda::do_z_step() +{ + digitalWrite(Z_STEP_PIN, HIGH); + delayMicroseconds(5); + digitalWrite(Z_STEP_PIN, LOW); +} + +inline void cartesian_dda::do_e_step() +{ + ext->step(); +} + +inline long cartesian_dda::calculate_feedrate_delay(float feedrate) +{ + + // Calculate delay between steps in microseconds. Here it is in English: + // (feedrate is in mm/minute, distance is in mm) + // 60000000.0*distance/feedrate = move duration in microseconds + // move duration/total_steps = time between steps for master axis. + + return round( (distance*60000000.0) / (feedrate*(float)total_steps) ); +} + +inline bool cartesian_dda::read_switch(byte pin) +{ + //dual read as crude debounce + + #if ENDSTOPS_INVERTING == 1 + return !digitalRead(pin) && !digitalRead(pin); + #else + return digitalRead(pin) && digitalRead(pin); + #endif +} + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h.dist b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h.dist new file mode 100644 index 00000000..2336fb56 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.h.dist @@ -0,0 +1,175 @@ +/* + * This class controls the movement of the RepRap machine. + * It implements a DDA in four dimensions, so the length of extruded + * filament is treated as a variable, just like X, Y, and Z. + * + * Adrian Bowyer 9 May 2009 + */ + +#ifndef CARTESIAN_DDA_H +#define CARTESIAN_DDA_H + +// Main class for moving the RepRap machine about + +class cartesian_dda +{ +private: + + extruder* ext; // The extruder I'm currently using - keep this up to date... + + FloatPoint units; // Factors for converting either mm or inches to steps + +// FloatPoint current_position; // Where the machine is + FloatPoint target_position; // Where it's going + FloatPoint delta_position; // The difference between the two + float distance; // How long the path is + + LongPoint current_steps; // Similar information as above in steps rather than units + LongPoint target_steps; + LongPoint delta_steps; + LongPoint dda_counter; // DDA error-accumulation variables + long t_scale; // When doing lots of t steps, scale them so the DDA doesn't spend for ever on them + + byte x_direction; // Am I going in the + or - direction? + byte y_direction; + byte z_direction; + byte e_direction; + byte f_direction; + + bool x_can_step; // Am I not at an endstop? Have I not reached the target? etc. + bool y_can_step; + bool z_can_step; + bool e_can_step; + bool f_can_step; + +// Variables for acceleration calculations + + long total_steps; // The number of steps to take along the longest movement axis + + long timestep; // microseconds + bool nullmove; // this move is zero length + bool real_move; // Flag to know if we've changed something physical + volatile bool live; // Flag for when we're plotting a line + +// Internal functions that need not concern the user + + // Take a single step + + void do_x_step(); + void do_y_step(); + void do_z_step(); + void do_e_step(); + + // Can this axis step? + + bool can_step(byte min_pin, byte max_pin, long current, long target, byte dir); + + // Read a limit switch + + bool read_switch(byte pin); + + // Work out the number of microseconds between steps + + long calculate_feedrate_delay(float feedrate); + + // Switch the steppers on and off + + void enable_steppers(); + void disable_steppers(); + + // Custom short delay function (microseconds) + + //void delayMicrosecondsInterruptible(unsigned int us); + + +public: + + cartesian_dda(); + + // Set where I'm going + + void set_target(const FloatPoint& p); + + // Start the DDA + + void dda_start(); + + // Do one step of the DDA + + void dda_step(); + + // Are we running at the moment? + + bool active(); + + // True for mm; false for inches + + void set_units(bool using_mm); + + // Record the selection of a new extruder + + void set_extruder(extruder* ex); +}; + +// Short functions inline to save memory; particularly useful in the Arduino + + +inline void cartesian_dda::set_extruder(extruder* ex) +{ + ext = ex; +} + +inline bool cartesian_dda::active() +{ + return live; +} + +inline void cartesian_dda::do_x_step() +{ + digitalWrite(X_STEP_PIN, HIGH); + delayMicroseconds(5); + digitalWrite(X_STEP_PIN, LOW); +} + +inline void cartesian_dda::do_y_step() +{ + digitalWrite(Y_STEP_PIN, HIGH); + delayMicroseconds(5); + digitalWrite(Y_STEP_PIN, LOW); +} + +inline void cartesian_dda::do_z_step() +{ + digitalWrite(Z_STEP_PIN, HIGH); + delayMicroseconds(5); + digitalWrite(Z_STEP_PIN, LOW); +} + +inline void cartesian_dda::do_e_step() +{ + ext->step(); +} + +inline long cartesian_dda::calculate_feedrate_delay(float feedrate) +{ + + // Calculate delay between steps in microseconds. Here it is in English: + // (feedrate is in mm/minute, distance is in mm) + // 60000000.0*distance/feedrate = move duration in microseconds + // move duration/total_steps = time between steps for master axis. + + return round( (distance*60000000.0) / (feedrate*(float)total_steps) ); +} + +inline bool cartesian_dda::read_switch(byte pin) +{ + //dual read as crude debounce + + #if ENDSTOPS_INVERTING == 1 + return !digitalRead(pin) && !digitalRead(pin); + #else + return digitalRead(pin) && digitalRead(pin); + #endif +} + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.pde b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.pde new file mode 100644 index 00000000..16bf2080 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/cartesian_dda.pde @@ -0,0 +1,477 @@ +#include <stdio.h> +#include "parameters.h" +#include "pins.h" +#include "extruder.h" +#include "vectors.h" +#include "cartesian_dda.h" + + +// Initialise X, Y and Z. The extruder is initialized +// separately. + +cartesian_dda::cartesian_dda() +{ + live = false; + nullmove = false; + + // Default is going forward + + x_direction = 1; + y_direction = 1; + z_direction = 1; + e_direction = 1; + f_direction = 1; + + // Default to the origin and not going anywhere + + target_position.x = 0.0; + target_position.y = 0.0; + target_position.z = 0.0; + target_position.e = 0.0; + target_position.f = SLOW_XY_FEEDRATE; + + // Set up the pin directions + + pinMode(X_STEP_PIN, OUTPUT); + pinMode(X_DIR_PIN, OUTPUT); + + pinMode(Y_STEP_PIN, OUTPUT); + pinMode(Y_DIR_PIN, OUTPUT); + + pinMode(Z_STEP_PIN, OUTPUT); + pinMode(Z_DIR_PIN, OUTPUT); + +#ifdef SANGUINO + pinMode(X_ENABLE_PIN, OUTPUT); + pinMode(Y_ENABLE_PIN, OUTPUT); + pinMode(Z_ENABLE_PIN, OUTPUT); +#endif + + //turn the motors off at the start. + + disable_steppers(); + +#if ENDSTOPS_MIN_ENABLED == 1 + pinMode(X_MIN_PIN, INPUT); + pinMode(Y_MIN_PIN, INPUT); + pinMode(Z_MIN_PIN, INPUT); +#endif + +#if ENDSTOPS_MAX_ENABLED == 1 + pinMode(X_MAX_PIN, INPUT); + pinMode(Y_MAX_PIN, INPUT); + pinMode(Z_MAX_PIN, INPUT); +#endif + + // Default units are mm + + set_units(true); +} + +// Switch between mm and inches + +void cartesian_dda::set_units(bool using_mm) +{ + if(using_mm) + { + units.x = X_STEPS_PER_MM; + units.y = Y_STEPS_PER_MM; + units.z = Z_STEPS_PER_MM; + units.e = E_STEPS_PER_MM; + units.f = 1.0; + } else + { + units.x = X_STEPS_PER_INCH; + units.y = Y_STEPS_PER_INCH; + units.z = Z_STEPS_PER_INCH; + units.e = E_STEPS_PER_INCH; + units.f = 1.0; + } +} + + +void cartesian_dda::set_target(const FloatPoint& p) +{ + target_position = p; + nullmove = false; + + //figure our deltas. + + delta_position = fabsv(target_position - where_i_am); + + // The feedrate values refer to distance in (X, Y, Z) space, so ignore e and f + // values unless they're the only thing there. + + FloatPoint squares = delta_position*delta_position; + distance = squares.x + squares.y + squares.z; + // If we are 0, only thing changing is e + if(distance <= 0.0) + distance = squares.e; + // If we are still 0, only thing changing is f + if(distance <= 0.0) + distance = squares.f; + distance = sqrt(distance); + + //set our steps current, target, and delta + + current_steps = to_steps(units, where_i_am); + target_steps = to_steps(units, target_position); + delta_steps = absv(target_steps - current_steps); + + // find the dominant axis. + // NB we ignore the f values here, as it takes no time to take a step in time :-) + + total_steps = max(delta_steps.x, delta_steps.y); + total_steps = max(total_steps, delta_steps.z); + total_steps = max(total_steps, delta_steps.e); + + // If we're not going anywhere, flag the fact + + if(total_steps == 0) + { + nullmove = true; + where_i_am = p; + return; + } + +#ifndef ACCELERATION_ON + current_steps.f = round(target_position.f); +#endif + + delta_steps.f = abs(target_steps.f - current_steps.f); + + // Rescale the feedrate so it doesn't take lots of steps to do + + t_scale = 1; + if(delta_steps.f > total_steps) + { + t_scale = delta_steps.f/total_steps; + if(t_scale >= 3) + { + target_steps.f = target_steps.f/t_scale; + current_steps.f = current_steps.f/t_scale; + delta_steps.f = abs(target_steps.f - current_steps.f); + if(delta_steps.f > total_steps) + total_steps = delta_steps.f; + } else + { + t_scale = 1; + total_steps = delta_steps.f; + } + } + + //what is our direction? + + x_direction = (target_position.x >= where_i_am.x); + y_direction = (target_position.y >= where_i_am.y); + z_direction = (target_position.z >= where_i_am.z); + e_direction = (target_position.e >= where_i_am.e); + f_direction = (target_position.f >= where_i_am.f); + + dda_counter.x = -total_steps/2; + dda_counter.y = dda_counter.x; + dda_counter.z = dda_counter.x; + dda_counter.e = dda_counter.x; + dda_counter.f = dda_counter.x; + + where_i_am = p; + + return; +} + + + +void cartesian_dda::dda_step() +{ + if(!live) + return; + + do + { + // ERIK NEW: + softPWM(); + + + x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction); + y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction); + z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction); + e_can_step = can_step(-1, -1, current_steps.e, target_steps.e, e_direction); + f_can_step = can_step(-1, -1, current_steps.f, target_steps.f, f_direction); + + real_move = false; + + if (x_can_step) + { + dda_counter.x += delta_steps.x; + + if (dda_counter.x > 0) + { + do_x_step(); + real_move = true; + dda_counter.x -= total_steps; + + if (x_direction) + current_steps.x++; + else + current_steps.x--; + } + } + + if (y_can_step) + { + dda_counter.y += delta_steps.y; + + if (dda_counter.y > 0) + { + do_y_step(); + real_move = true; + dda_counter.y -= total_steps; + + if (y_direction) + current_steps.y++; + else + current_steps.y--; + } + } + + if (z_can_step) + { + dda_counter.z += delta_steps.z; + + if (dda_counter.z > 0) + { + do_z_step(); + real_move = true; + dda_counter.z -= total_steps; + + if (z_direction) + current_steps.z++; + else + current_steps.z--; + } + } + + if (e_can_step) + { + dda_counter.e += delta_steps.e; + + if (dda_counter.e > 0) + { + do_e_step(); + real_move = true; + dda_counter.e -= total_steps; + + if (e_direction) + current_steps.e++; + else + current_steps.e--; + } + } + + if (f_can_step) + { + dda_counter.f += delta_steps.f; + + if (dda_counter.f > 0) + { + dda_counter.f -= total_steps; + if (f_direction) + current_steps.f++; + else + current_steps.f--; + } + } + + + // wait for next step. + // Use milli- or micro-seconds, as appropriate + // If the only thing that changed was f keep looping + + if(real_move) + { + if(t_scale > 1) + timestep = t_scale*current_steps.f; + else + timestep = current_steps.f; + timestep = calculate_feedrate_delay((float) timestep); + setTimer(timestep); + } + } while (!real_move && f_can_step); + + live = (x_can_step || y_can_step || z_can_step || e_can_step || f_can_step); + +// Wrap up at the end of a line + + if(!live) + { + disable_steppers(); + setTimer(DEFAULT_TICK); + } + +} + + +// Run the DDA + +void cartesian_dda::dda_start() +{ + // Set up the DDA + //sprintf(debugstring, "%d %d", x_direction, nullmove); + + if(nullmove) + return; + + //set our direction pins as well +#if INVERT_X_DIR == 1 + digitalWrite(X_DIR_PIN, !x_direction); +#else + digitalWrite(X_DIR_PIN, x_direction); +#endif + +#if INVERT_Y_DIR == 1 + digitalWrite(Y_DIR_PIN, !y_direction); +#else + digitalWrite(Y_DIR_PIN, y_direction); +#endif + +#if INVERT_Z_DIR == 1 + digitalWrite(Z_DIR_PIN, !z_direction); +#else + digitalWrite(Z_DIR_PIN, z_direction); +#endif +delayMicroseconds(110);//ERIK: FIXME Dit kost misschien te veel tijd maar nu voldoen we wel aan de specs van de driver IC. + if(e_direction) + ext->set_direction(1); + else + ext->set_direction(0); + + //turn on steppers to start moving =) + + enable_steppers(); + // extcount = 0; + + setTimer(DEFAULT_TICK); + live = true; +} + + +bool cartesian_dda::can_step(byte min_pin, byte max_pin, long current, long target, byte dir) +{ + + //stop us if we're on target + + if (target == current) + return false; + +#if ENDSTOPS_MIN_ENABLED == 1 + + //stop us if we're home and still going + + else if(min_pin >= 0) + { + if (read_switch(min_pin) && !dir) + return false; + } +#endif + +#if ENDSTOPS_MAX_ENABLED == 1 + + //stop us if we're at max and still going + + else if(max_pin >= 0) + { + if (read_switch(max_pin) && dir) + return false; + } +#endif + + // All OK - we can step + + return true; +} + + +void cartesian_dda::enable_steppers() +{ +#ifdef SANGUINO + if(delta_steps.x) + digitalWrite(X_ENABLE_PIN, LOW);// ERIK: dit is een inverted boardje. + if(delta_steps.y) + digitalWrite(Y_ENABLE_PIN, LOW);// ERIK dit zelfbouw board luistert niet, maar toch... + if(delta_steps.z) +//#ifdef MOTHERBOARD_ERIK2 +// digitalWrite(Z_ENABLE_PIN, ENABLE_ON); +//#else + digitalWrite(Z_ENABLE_PIN, ENABLE_ON); +//#endif + if(delta_steps.e) + ext->enableStep(); +#endif +} + + + +void cartesian_dda::disable_steppers() +{ +#ifdef SANGUINO + //disable our steppers + // ERIK: let op: X_ENABLE_PIN == Y_ENABLE_PIN + //digitalWrite(X_ENABLE_PIN, ENABLE_ON);// ERIK: dit is een inverted board + //digitalWrite(Y_ENABLE_PIN, ENABLE_ON);// ERIK: dit is een inverted board, luistert alleen niet naar enable.. + digitalWrite(Z_ENABLE_PIN, !ENABLE_ON); + + // Disabling the extrude stepper causes the backpressure to + // turn the motor the wrong way. Leave it on. + + //ext->disableStep(); +#endif +} + +/* + +void cartesian_dda::delayMicrosecondsInterruptible(unsigned int us) +{ + +#if F_CPU >= 16000000L + // for the 16 MHz clock on most Arduino boards + + // for a one-microsecond delay, simply return. the overhead + // of the function call yields a delay of approximately 1 1/8 us. + if (--us == 0) + return; + + // the following loop takes a quarter of a microsecond (4 cycles) + // per iteration, so execute it four times for each microsecond of + // delay requested. + us <<= 2; + + // account for the time taken in the preceeding commands. + us -= 2; +#else + // for the 8 MHz internal clock on the ATmega168 + + // for a one- or two-microsecond delay, simply return. the overhead of + // the function calls takes more than two microseconds. can't just + // subtract two, since us is unsigned; we'd overflow. + if (--us == 0) + return; + if (--us == 0) + return; + + // the following loop takes half of a microsecond (4 cycles) + // per iteration, so execute it twice for each microsecond of + // delay requested. + us <<= 1; + + // partially compensate for the time taken by the preceeding commands. + // we can't subtract any more than this or we'd overflow w/ small delays. + us--; +#endif + + // busy wait + __asm__ __volatile__ ( + "1: sbiw %0,1" "\n\t" // 2 cycles + "brne 1b" : "=w" (us) : "0" (us) // 2 cycles + ); +} +*/ diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/download-copy b/trunk/users/erik/FiveD_GCode_Interpreter/download-copy new file mode 100755 index 00000000..c3a47867 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/download-copy @@ -0,0 +1,7 @@ +#!/bin/sh +cp cartesian_dda.h.dist cartesian_dda.h +cp extruder.h.dist extruder.h +cp parameters.h.dist parameters.h +cp pins.h.dist pins.h +cp vectors.h.dist vectors.h + diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/eriks_differences.txt b/trunk/users/erik/FiveD_GCode_Interpreter/eriks_differences.txt new file mode 100644 index 00000000..b1611015 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/eriks_differences.txt @@ -0,0 +1,587 @@ +Index: process_g_code.pde +=================================================================== +--- process_g_code.pde (revision 3145) ++++ process_g_code.pde (working copy) +@@ -214,6 +214,19 @@ + { + last_gcode_g = gc.G; /* remember this for future instructions */ + fp = where_i_am; ++#ifdef MODIFIER_PIN ++ modifier = analogRead(MODIFIER_PIN); // Erik ++if(((modifier - modifier_old) > 3) || ((modifier - modifier_old) < -3)) ++{ ++ Serial.print("Modifier: "); ++ Serial.println(modifier); ++ modifier_old = modifier; ++} ++#endif ++ // Debug Erik: ++ //Serial.print("Modifier: "); ++ //Serial.println(modifier/512); ++ + if (abs_mode) + { + if (gc.seen & GCODE_X) +@@ -223,7 +236,28 @@ + if (gc.seen & GCODE_Z) + fp.z = gc.Z; + if (gc.seen & GCODE_E) +- fp.e = gc.E; ++#ifdef MODIFIER_PIN ++if(gc.E<0) ++{ ++ Serial.print("Reversing: "); ++ Serial.print(gc.E); ++ Serial.print(", extr pos bef: "); ++ Serial.print(fp.e); ++ fp.e += gc.E*(modifier/512); // Erik ++ Serial.print(", extr pos after: "); ++ Serial.println(fp.e); ++} else ++{ ++ fp.e += gc.E*(modifier/512); // Erik ++ Serial.print("fp.e is: "); ++ Serial.println(fp.e); ++} ++ // Erik: Make forced incremental ++#else ++ fp.e += gc.E; // Erik ++// fp.e += gc.E; //ERIK: normaal niet aditive! ++ ++#endif + } + else + { +@@ -234,13 +268,20 @@ + if (gc.seen & GCODE_Z) + fp.z += gc.Z; + if (gc.seen & GCODE_E) +- fp.e += gc.E; ++#ifndef MODIFIER_PIN ++ fp.e += gc.E; ++#else ++ fp.e += gc.E*(modifier/512); // Erik ++#endif + } + + // Get feedrate if supplied - feedrates are always absolute??? + if ( gc.seen & GCODE_F ) +- fp.f = gc.F; +- ++//#ifdef MODIFIER_PIN ++ fp.f = gc.F*(modifier_speed/512); // ERIK ++////#else ++// fp.f = gc.F; ++//#endif + // Process the buffered move commands first + // If we get one, return immediately + +Index: FiveD_GCode_Interpreter.pde +=================================================================== +--- FiveD_GCode_Interpreter.pde (revision 3145) ++++ FiveD_GCode_Interpreter.pde (working copy) +@@ -18,7 +18,6 @@ + #include "cartesian_dda.h" + + // Inline interrupt control functions +- + inline void enableTimerInterrupt() + { + TIMSK1 |= (1<<OCIE1A); +@@ -38,7 +37,6 @@ + { + TCNT2 = 0; + } +- + char debugstring[COMMAND_SIZE]; + + // Maintain a list of extruders... +@@ -68,7 +66,6 @@ + + + // Our interrupt function +- + SIGNAL(SIG_OUTPUT_COMPARE1A) + { + disableTimerInterrupt(); +@@ -81,9 +78,18 @@ + enableTimerInterrupt(); + } + ++//ERIK ++#ifdef MODIFIER_PIN ++double modifier = 500.0; ++double modifier_old = 1.0; ++#endif ++double modifier_speed = 512.0; //ERIK ++ + void setup() + { ++ + disableTimerInterrupt(); ++ // This screws with PWM :( + setupTimerInterrupt(); + debugstring[0] = 0; + extruder_in_use = 0; +@@ -105,12 +111,39 @@ + where_i_am.e = 0.0; + where_i_am.f = SLOW_XY_FEEDRATE; + +- Serial.begin(19200); ++ Serial.begin(57600); + Serial.println("start"); + setTimer(DEFAULT_TICK); + enableTimerInterrupt(); ++ ++ // ERIK: ++#ifdef MODIFIER_PIN ++ pinMode(MODIFIER_PIN,INPUT); ++#endif ++ + } + ++byte softPWMduty = 0; ++byte softPWMcount = 0; ++int PWMstate = HIGH; ++static void softPWM() ++{ ++ if(softPWMduty == softPWMcount) ++ { ++ PWMstate = LOW; ++ } ++ digitalWrite(12,PWMstate); ++ ++ if(softPWMcount<254) ++ softPWMcount++; ++ else ++ { ++ PWMstate = HIGH; ++ softPWMcount=0; ++ } ++} ++ ++ + void loop() + { + manage_all_extruders(); +@@ -177,7 +210,6 @@ + //****************************************************************************************** + + // Interrupt functions +- + void setupTimerInterrupt() + { + //clear the registers +@@ -199,7 +231,7 @@ + TCCR1A &= ~(1<<COM1B0); + + //start off with a slow frequency. +- setTimerResolution(4); ++ setTimerResolution(5); + setTimerCeiling(65535); + } + +Index: cartesian_dda.pde +=================================================================== +--- cartesian_dda.pde (revision 3145) ++++ cartesian_dda.pde (working copy) +@@ -188,6 +188,10 @@ + + do + { ++ // ERIK NEW: ++ softPWM(); ++ ++ + x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction); + y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction); + z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction); +@@ -335,6 +339,7 @@ + #else + digitalWrite(Z_DIR_PIN, z_direction); + #endif ++delayMicroseconds(110);//ERIK: FIXME Dit kost misschien te veel tijd maar nu voldoen we wel aan de specs van de driver IC. + if(e_direction) + ext->set_direction(1); + else +@@ -343,7 +348,6 @@ + //turn on steppers to start moving =) + + enable_steppers(); +- + // extcount = 0; + + setTimer(DEFAULT_TICK); +@@ -391,11 +395,15 @@ + { + #ifdef SANGUINO + if(delta_steps.x) +- digitalWrite(X_ENABLE_PIN, ENABLE_ON); ++ digitalWrite(X_ENABLE_PIN, LOW);// ERIK: dit is een inverted boardje. + if(delta_steps.y) +- digitalWrite(Y_ENABLE_PIN, ENABLE_ON); ++ digitalWrite(Y_ENABLE_PIN, LOW);// ERIK dit zelfbouw board luistert niet, maar toch... + if(delta_steps.z) ++//#ifdef MOTHERBOARD_ERIK2 ++// digitalWrite(Z_ENABLE_PIN, ENABLE_ON); ++//#else + digitalWrite(Z_ENABLE_PIN, ENABLE_ON); ++//#endif + if(delta_steps.e) + ext->enableStep(); + #endif +@@ -407,8 +415,9 @@ + { + #ifdef SANGUINO + //disable our steppers +- digitalWrite(X_ENABLE_PIN, !ENABLE_ON); +- digitalWrite(Y_ENABLE_PIN, !ENABLE_ON); ++ // ERIK: let op: X_ENABLE_PIN == Y_ENABLE_PIN ++ //digitalWrite(X_ENABLE_PIN, ENABLE_ON);// ERIK: dit is een inverted board ++ //digitalWrite(Y_ENABLE_PIN, ENABLE_ON);// ERIK: dit is een inverted board, luistert alleen niet naar enable.. + digitalWrite(Z_ENABLE_PIN, !ENABLE_ON); + + // Disabling the extrude stepper causes the backpressure to +Index: extruder.pde +=================================================================== +--- extruder.pde (revision 3145) ++++ extruder.pde (working copy) +@@ -1,4 +1,3 @@ +- + #include "parameters.h" + #include "pins.h" + #include "ThermistorTable.h" +@@ -24,20 +23,21 @@ + //setup our pins + pinMode(motor_dir_pin, OUTPUT); + pinMode(motor_speed_pin, OUTPUT); +- pinMode(heater_pin, OUTPUT); ++ //NOT NEEDED: pinMode(heater_pin, OUTPUT); + + pinMode(temp_pin, INPUT); +- pinMode(valve_dir_pin, OUTPUT); +- pinMode(valve_en_pin, OUTPUT); ++ //pinMode(valve_dir_pin, OUTPUT); ++ //pinMode(valve_en_pin, OUTPUT); + + //initialize values + digitalWrite(motor_dir_pin, EXTRUDER_FORWARD); +- +- analogWrite(heater_pin, 0); +- analogWrite(motor_speed_pin, 0); +- digitalWrite(valve_dir_pin, false); +- digitalWrite(valve_en_pin, 0); ++ analogWrite(heater_pin, 64); ++ //digitalWrite(heater_pin, LOW);//ERIK: changed to digital, LOW ++ digitalWrite(motor_speed_pin, 0);// ERIK: changed to digital ++ //digitalWrite(valve_dir_pin, false); ++ ///digitalWrite(valve_en_pin, 0); + ++ + // The step enable pin and the fan pin are the same... + // We can have one, or the other, but not both + +@@ -51,12 +51,26 @@ + analogWrite(fan_pin, 0); + } + +- //these our the default values for the extruder. ++// From makerbot branch: ++#if TEMP_PID ++ temp_iState = 0; ++ temp_dState = 0; ++ temp_pGain = TEMP_PID_PGAIN; ++ temp_iGain = TEMP_PID_IGAIN; ++ temp_dGain = TEMP_PID_DGAIN; ++ ++ temp_pid_update_windup(); ++#endif ++ ++ temp_control_enabled = true; ++ current_temperature = 0; ++ target_temperature = 100; // target_celcius ++ max_temperature = 0; // max_celcius ++ ++ //these our the default values for the extruder. + e_speed = 0; +- target_celsius = 0; +- max_celsius = 0; +- heater_low = 64; +- heater_high = 255; ++ byte heater_low = 64; ++ byte heater_high = 255; + heater_current = 0; + valve_open = false; + +@@ -69,7 +83,7 @@ + e_direction = EXTRUDER_FORWARD; + + //default to cool +- set_temperature(target_celsius); ++ set_temperature(target_temperature); + } + + +@@ -77,7 +91,7 @@ + { + count = 0; + oldT = get_temperature(); +- while (get_temperature() < target_celsius - HALF_DEAD_ZONE) ++ while (get_temperature() < target_temperature - HALF_DEAD_ZONE) + { + manage_all_extruders(); + count++; +@@ -95,32 +109,8 @@ + return 0; + } + +-/* +-byte extruder::wait_till_cool() +-{ +- count = 0; +- oldT = get_temperature(); +- while (get_temperature() > target_celsius + HALF_DEAD_ZONE) +- { +- manage_all_extruders(); +- count++; +- if(count > 20) +- { +- newT = get_temperature(); +- if(newT < oldT) +- oldT = newT; +- else +- return 1; +- count = 0; +- } +- delay(1000); +- } +- return 0; +-} +-*/ + + +- + void extruder::valve_set(bool open, int millis) + { + wait_for_temperature(); +@@ -134,11 +124,11 @@ + + void extruder::set_temperature(int temp) + { +- target_celsius = temp; +- max_celsius = (temp*11)/10; ++ target_temperature = temp; ++ max_temperature = (temp*11)/10; + + // If we've turned the heat off, we might as well disable the extrude stepper +- if(target_celsius < 1) ++ if(target_temperature < 0) + ex[extruder_in_use]->disableStep(); + } + +@@ -199,82 +189,140 @@ + return raw; + } + ++ ++#if TEMP_PID ++int extruder::temp_update(int dt) ++{ ++ int output; ++ int error; ++ float pTerm, iTerm, dTerm; ++ ++ if (temp_control_enabled) { ++ error = target_temperature - current_temperature; ++ ++ pTerm = temp_pGain * error; ++ ++ temp_iState += error; ++ temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); ++ iTerm = temp_iGain * temp_iState; ++ ++ dTerm = temp_dGain * (current_temperature - temp_dState); ++ temp_dState = current_temperature; ++ ++ output = pTerm + iTerm - dTerm; ++ output = constrain(output, 0, 255); ++ } else { ++ output = 0; ++ } ++ return output; ++} ++ ++void extruder::temp_pid_update_windup() ++{ ++ temp_iState_min = -TEMP_PID_INTEGRAL_DRIVE_MAX/temp_iGain; ++ temp_iState_max = TEMP_PID_INTEGRAL_DRIVE_MAX/temp_iGain; ++} ++ ++#else ++ ++int extruder::temp_update(int dt) ++{ ++ int output; ++ ++ if (temp_control_enabled) { ++ //put the heater into high mode if we're not at our target. ++ if (current_temperature < target_temperature) ++ output = heater_high; ++ //put the heater on low if we're at our target. ++ else if (current_temperature < max_temperature) ++ output = heater_low; ++ //turn the heater off if we're above our max. ++ else ++ output = 0; ++ } else { ++ output = 0; ++ } ++ return output; ++} ++#endif /* TEMP_PID */ ++ ++ ++/*void extruder::manage() ++{ ++ //int output = random(255); ++// analogWrite(12,128); ++ // ++ //digitalWrite(12,(output > 64)?HIGH:LOW); ++ //Serial.print("!"); ++ //delay(200); ++} ++*/ + /*! ++Manages motor and heater based on measured temperature: ++o If temp is too low, don't start the motor ++o Adjust the heater power to keep the temperature at the target ++*/ ++// NEW ++void extruder::manage() ++{ ++ int output, dt; ++ unsigned long time; ++ ++ //make sure we know what our temp is. ++ current_temperature = get_temperature(); ++ ++ // ignoring millis rollover for now ++ time = millis(); ++ dt = time - temp_prev_time; ++ ++ if (dt > TEMP_UPDATE_INTERVAL) ++ { ++ temp_prev_time = time; ++ output = temp_update(dt); ++ //digitalWrite(DEBUG_PIN, (output > 0)?HIGH:LOW); ++// analogWrite(12, output); ++// delay(200); ++ digitalWrite(heater_pin, (output > 128)?HIGH:LOW); ++ softPWMduty = output; ++// softPWM(); // deze moet gewoon aan of uit... ++ } ++} ++ ++ ++/*! + Manages extruder functions to keep temps, speeds etc + at the set levels. Should be called only by manage_all_extruders(), + which should be called in all non-trivial loops. + o If temp is too low, don't start the motor + o Adjust the heater power to keep the temperature at the target + */ ++// OLD ++/* + void extruder::manage() + { + //make sure we know what our temp is. + int current_celsius = get_temperature(); +- byte newheat = 0; ++ int newheat = 0; + + //put the heater into high mode if we're not at our target. + if (current_celsius < target_celsius) +- newheat = heater_high; ++ newheat = 255; + //put the heater on low if we're at our target. + else if (current_celsius < max_celsius) +- newheat = heater_low; ++ newheat = 64; + + // Only update heat if it changed + if (heater_current != newheat) { + heater_current = newheat; +- analogWrite(heater_pin, heater_current); ++// analogWrite(heater_pin, newheat); // STandard ++ analogWrite(12, newheat); // STandard ++// digitalWrite(heater_pin, heater_current);//ERIK + } + } + ++*/ + +-#if 0 +-void extruder::set_speed(float sp) +-{ +- // DC motor? +- if(step_en_pin < 0) +- { +- e_speed = (byte)sp; +- if(e_speed > 0) +- wait_for_temperature(); +- analogWrite(motor_speed_pin, e_speed); +- return; +- } +- +- // No - stepper +- disableTimerInterrupt(); +- +- if(sp <= 1.0e-4) +- { +- disableStep(); +- e_speed = 0; // Just use this as a flag +- return; +- } else +- { +- wait_for_temperature(); +- enableStep(); +- e_speed = 1; +- } +- +- extrude_step_count = 0; +- +- float milliseconds_per_step = 60000.0/(E_STEPS_PER_MM*sp); +- long thousand_ticks_per_step = 4*(long)(milliseconds_per_step); +- setupTimerInterrupt(); +- setTimer(thousand_ticks_per_step); +- enableTimerInterrupt(); +-} + +-void extruder::interrupt() +-{ +- if(!e_speed) +- return; +- extrude_step_count++; +- if(extrude_step_count > 1000) +- { +- step(); +- extrude_step_count = 0; +- } +-} ++// NOT USED + +-#endif + +Index: ThermistorTable.h +=================================================================== +--- ThermistorTable.h (revision 3145) ++++ ThermistorTable.h (working copy) +@@ -4,8 +4,6 @@ + // Uncomment the next line if you are using a thermistor; leave it if you have a thermocouple + //#define USE_THERMISTOR + +-// How many temperature samples to take for an average. each sample takes about 100 usecs. +-#define TEMPERATURE_SAMPLES 3 + + // How accurately do we maintain the temperature? + #define HALF_DEAD_ZONE 5 diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h new file mode 100644 index 00000000..26af9af6 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h @@ -0,0 +1,153 @@ +// Class for controlling each extruder +// +// Adrian Bowyer 14 May 2009 + +#ifndef EXTRUDER_H +#define EXTRUDER_H + +#define EXTRUDER_FORWARD LOW +#define EXTRUDER_REVERSE HIGH + +#define EXTRUDER_COUNT 1 + +void manage_all_extruders(); + +class extruder +{ +private: +// NEW ERIK +#if TEMP_PID + float temp_pGain; + float temp_iGain; + float temp_dGain; + + int temp_dState; + long temp_iState; + float temp_iState_max; // set in update_windup + float temp_iState_min; // set in update_windup +#endif + // NEW ERIK + unsigned long temp_prev_time; // ms + bool temp_control_enabled; + int temp_update(int dt); +//these our the default values for the extruder. + byte e_speed; + int target_temperature; + int max_temperature; + int current_temperature; + byte heater_low; + byte heater_high; + byte heater_current; + int extrude_step_count; + +// These are used for temperature control + byte count ; + int oldT, newT; + +//this is for doing encoder based extruder control + int rpm; + long e_delay; + int error; + int last_extruder_error; + int error_delta; + bool e_direction; + bool valve_open; + +// The pins we control + byte motor_dir_pin, motor_speed_pin, heater_pin, fan_pin, temp_pin, valve_dir_pin, valve_en_pin, step_en_pin; + +public: + + extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin); + void wait_for_temperature(); + //byte wait_till_cool(); + byte wait_till_hot(); + void temperature_error(); + void valve_set(bool open, int millis); + void set_direction(bool direction); +// void set_speed(float es); + void set_cooler(byte e_speed); + void set_temperature(int temp); + int get_temperature(); + int sample_temperature(byte pin); + void manage(); + + // NEW ERIK + void temp_pid_update_windup(); + + // Interrupt setup and handling functions for stepper-driven extruders + + void interrupt(); + void step(); + + void enableStep(); + void disableStep(); + +}; + +inline void extruder::enableStep() +{ + if(step_en_pin < 0) + return; + digitalWrite(step_en_pin, LOW); //ERIK // Should be ENABLE_ON, but I have a mix of controllers +} + +inline void extruder::disableStep() +{ + if(step_en_pin < 0) + return; + digitalWrite(step_en_pin, HIGH);//ERIK +// Serial.println("disableStep"); +} + +inline void extruder::step() +{ +// for(int a=0;a<50;a++) +// { +// digitalWrite(motor_dir_pin,digitalRead(motor_dir_pin)); + digitalWrite(motor_speed_pin, LOW);//ERIK + // + delayMicroseconds(5); + digitalWrite(motor_speed_pin, HIGH); //ERIK + //delay(1000); +// } +// Serial.print("step. Dir pin: "); + //if(digitalRead(motor_dir_pin)) +// Serial.println(motor_dir_pin); + //else Serial.println(" reverse"); +} + +inline void extruder::temperature_error() +{ + Serial.print("E: "); + Serial.println(get_temperature()); +} + +//warmup if we're too cold; cool down if we're too hot +inline void extruder::wait_for_temperature() +{ +/* + if(wait_till_cool()) + { + temperature_error(); + return; + } +*/ + if(wait_till_hot()) + temperature_error(); +} + +inline void extruder::set_direction(bool dir) +{ + e_direction = dir; + digitalWrite(motor_dir_pin, !e_direction); +} + +inline void extruder::set_cooler(byte sp) +{ + if(step_en_pin >= 0) // Step enable conflicts with the fan + return; + analogWrite(fan_pin, sp); +} + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h.dist b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h.dist new file mode 100644 index 00000000..da00959b --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.h.dist @@ -0,0 +1,125 @@ + +// Class for controlling each extruder +// +// Adrian Bowyer 14 May 2009 + +#ifndef EXTRUDER_H +#define EXTRUDER_H + +#define EXTRUDER_FORWARD true +#define EXTRUDER_REVERSE false + +#define EXTRUDER_COUNT 1 + +void manage_all_extruders(); + +class extruder +{ +private: + +//these our the default values for the extruder. + byte e_speed; + int target_celsius; + int max_celsius; + byte heater_low; + byte heater_high; + byte heater_current; + int extrude_step_count; + +// These are used for temperature control + byte count ; + int oldT, newT; + +//this is for doing encoder based extruder control + int rpm; + long e_delay; + int error; + int last_extruder_error; + int error_delta; + bool e_direction; + bool valve_open; + +// The pins we control + byte motor_dir_pin, motor_speed_pin, heater_pin, fan_pin, temp_pin, valve_dir_pin, valve_en_pin, step_en_pin; + +public: + + extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin); + void wait_for_temperature(); + //byte wait_till_cool(); + byte wait_till_hot(); + void temperature_error(); + void valve_set(bool open, int millis); + void set_direction(bool direction); +// void set_speed(float es); + void set_cooler(byte e_speed); + void set_temperature(int temp); + int get_temperature(); + int sample_temperature(byte pin); + void manage(); + + // Interrupt setup and handling functions for stepper-driven extruders + + void interrupt(); + void step(); + + void enableStep(); + void disableStep(); + +}; + +inline void extruder::enableStep() +{ + if(step_en_pin < 0) + return; + digitalWrite(step_en_pin, ENABLE_ON); // Should be ENABLE_ON, but I have a mix of controllers +} + +inline void extruder::disableStep() +{ + if(step_en_pin < 0) + return; + digitalWrite(step_en_pin, !ENABLE_ON); +} + +inline void extruder::step() +{ + digitalWrite(motor_speed_pin, HIGH); + delayMicroseconds(5); + digitalWrite(motor_speed_pin, LOW); +} + +inline void extruder::temperature_error() +{ + Serial.print("E: "); + Serial.println(get_temperature()); +} + +//warmup if we're too cold; cool down if we're too hot +inline void extruder::wait_for_temperature() +{ +/* + if(wait_till_cool()) + { + temperature_error(); + return; + } +*/ + if(wait_till_hot()) + temperature_error(); +} + +inline void extruder::set_direction(bool dir) +{ + e_direction = dir; + digitalWrite(motor_dir_pin, e_direction); +} + +inline void extruder::set_cooler(byte sp) +{ + if(step_en_pin >= 0) // Step enable conflicts with the fan + return; + analogWrite(fan_pin, sp); +} + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/extruder.pde b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.pde new file mode 100644 index 00000000..b6639ed4 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/extruder.pde @@ -0,0 +1,328 @@ +#include "parameters.h" +#include "pins.h" +#include "ThermistorTable.h" +#include "extruder.h" + +void manage_all_extruders() +{ + for(byte i = 0; i < EXTRUDER_COUNT; i++) + ex[i]->manage(); +} + +extruder::extruder(byte md_pin, byte ms_pin, byte h_pin, byte f_pin, byte t_pin, byte vd_pin, byte ve_pin, byte se_pin) +{ + motor_dir_pin = md_pin; + motor_speed_pin = ms_pin; + heater_pin = h_pin; + fan_pin = f_pin; + temp_pin = t_pin; + valve_dir_pin = vd_pin; + valve_en_pin = ve_pin; + step_en_pin = se_pin; + + //setup our pins + pinMode(motor_dir_pin, OUTPUT); + pinMode(motor_speed_pin, OUTPUT); + //NOT NEEDED: pinMode(heater_pin, OUTPUT); + + pinMode(temp_pin, INPUT); + //pinMode(valve_dir_pin, OUTPUT); + //pinMode(valve_en_pin, OUTPUT); + + //initialize values + digitalWrite(motor_dir_pin, EXTRUDER_FORWARD); + analogWrite(heater_pin, 64); + //digitalWrite(heater_pin, LOW);//ERIK: changed to digital, LOW + digitalWrite(motor_speed_pin, 0);// ERIK: changed to digital + //digitalWrite(valve_dir_pin, false); + ///digitalWrite(valve_en_pin, 0); + + +// The step enable pin and the fan pin are the same... +// We can have one, or the other, but not both + + if(step_en_pin >= 0) + { + pinMode(step_en_pin, OUTPUT); + disableStep(); + } else + { + pinMode(fan_pin, OUTPUT); + analogWrite(fan_pin, 0); + } + +// From makerbot branch: +#if TEMP_PID + temp_iState = 0; + temp_dState = 0; + temp_pGain = TEMP_PID_PGAIN; + temp_iGain = TEMP_PID_IGAIN; + temp_dGain = TEMP_PID_DGAIN; + + temp_pid_update_windup(); +#endif + + temp_control_enabled = true; + current_temperature = 0; + target_temperature = 100; // target_celcius + max_temperature = 0; // max_celcius + + //these our the default values for the extruder. + e_speed = 0; + byte heater_low = 64; + byte heater_high = 255; + heater_current = 0; + valve_open = false; + +//this is for doing encoder based extruder control + rpm = 0; + e_delay = 0; + error = 0; + last_extruder_error = 0; + error_delta = 0; + e_direction = EXTRUDER_FORWARD; + + //default to cool + set_temperature(target_temperature); +} + + +byte extruder::wait_till_hot() +{ + count = 0; + oldT = get_temperature(); + while (get_temperature() < target_temperature - HALF_DEAD_ZONE) + { + manage_all_extruders(); + count++; + if(count > 20) + { + newT = get_temperature(); + if(newT > oldT) + oldT = newT; + else + return 1; + count = 0; + } + delay(1000); + } + return 0; +} + + + +void extruder::valve_set(bool open, int millis) +{ + wait_for_temperature(); + valve_open = open; + digitalWrite(valve_dir_pin, open); + digitalWrite(valve_en_pin, 1); + delay(millis); + digitalWrite(valve_en_pin, 0); +} + + +void extruder::set_temperature(int temp) +{ + target_temperature = temp; + max_temperature = (temp*11)/10; + + // If we've turned the heat off, we might as well disable the extrude stepper + if(target_temperature < 0) + ex[extruder_in_use]->disableStep(); +} + +/** +* Samples the temperature and converts it to degrees celsius. +* Returns degrees celsius. +*/ +int extruder::get_temperature() +{ +#ifdef USE_THERMISTOR + int raw = sample_temperature(temp_pin); + + int celsius = 0; + byte i; + + for (i=1; i<NUMTEMPS; i++) + { + if (temptable[i][0] > raw) + { + celsius = temptable[i-1][1] + + (raw - temptable[i-1][0]) * + (temptable[i][1] - temptable[i-1][1]) / + (temptable[i][0] - temptable[i-1][0]); + + break; + } + } + + // Overflow: Set to last value in the table + if (i == NUMTEMPS) celsius = temptable[i-1][1]; + // Clamp to byte + if (celsius > 255) celsius = 255; + else if (celsius < 0) celsius = 0; + + return celsius; +#else + return ( 5.0 * sample_temperature(temp_pin) * 100.0) / 1024.0; +#endif +} + + + +/* +* This function gives us an averaged sample of the analog temperature pin. +*/ +int extruder::sample_temperature(byte pin) +{ + int raw = 0; + + //read in a certain number of samples + for (byte i=0; i<TEMPERATURE_SAMPLES; i++) + raw += analogRead(pin); + + //average the samples + raw = raw/TEMPERATURE_SAMPLES; + + //send it back. + return raw; +} + + +#if TEMP_PID +int extruder::temp_update(int dt) +{ + int output; + int error; + float pTerm, iTerm, dTerm; + + if (temp_control_enabled) { + error = target_temperature - current_temperature; + + pTerm = temp_pGain * error; + + temp_iState += error; + temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); + iTerm = temp_iGain * temp_iState; + + dTerm = temp_dGain * (current_temperature - temp_dState); + temp_dState = current_temperature; + + output = pTerm + iTerm - dTerm; + output = constrain(output, 0, 255); + } else { + output = 0; + } + return output; +} + +void extruder::temp_pid_update_windup() +{ + temp_iState_min = -TEMP_PID_INTEGRAL_DRIVE_MAX/temp_iGain; + temp_iState_max = TEMP_PID_INTEGRAL_DRIVE_MAX/temp_iGain; +} + +#else + +int extruder::temp_update(int dt) +{ + int output; + + if (temp_control_enabled) { + //put the heater into high mode if we're not at our target. + if (current_temperature < target_temperature) + output = heater_high; + //put the heater on low if we're at our target. + else if (current_temperature < max_temperature) + output = heater_low; + //turn the heater off if we're above our max. + else + output = 0; + } else { + output = 0; + } + return output; +} +#endif /* TEMP_PID */ + + +/*void extruder::manage() +{ + //int output = random(255); +// analogWrite(12,128); + // + //digitalWrite(12,(output > 64)?HIGH:LOW); + //Serial.print("!"); + //delay(200); +} +*/ +/*! +Manages motor and heater based on measured temperature: +o If temp is too low, don't start the motor +o Adjust the heater power to keep the temperature at the target +*/ +// NEW +void extruder::manage() +{ + int output, dt; + unsigned long time; + + //make sure we know what our temp is. + current_temperature = get_temperature(); + + // ignoring millis rollover for now + time = millis(); + dt = time - temp_prev_time; + + if (dt > TEMP_UPDATE_INTERVAL) + { + temp_prev_time = time; + output = temp_update(dt); + //digitalWrite(DEBUG_PIN, (output > 0)?HIGH:LOW); +// analogWrite(12, output); +// delay(200); + digitalWrite(heater_pin, (output > 128)?HIGH:LOW); + softPWMduty = output; +// softPWM(); // deze moet gewoon aan of uit... + } +} + + +/*! + Manages extruder functions to keep temps, speeds etc + at the set levels. Should be called only by manage_all_extruders(), + which should be called in all non-trivial loops. + o If temp is too low, don't start the motor + o Adjust the heater power to keep the temperature at the target + */ +// OLD +/* +void extruder::manage() +{ + //make sure we know what our temp is. + int current_celsius = get_temperature(); + int newheat = 0; + + //put the heater into high mode if we're not at our target. + if (current_celsius < target_celsius) + newheat = 255; + //put the heater on low if we're at our target. + else if (current_celsius < max_celsius) + newheat = 64; + + // Only update heat if it changed + if (heater_current != newheat) { + heater_current = newheat; +// analogWrite(heater_pin, newheat); // STandard + analogWrite(12, newheat); // STandard +// digitalWrite(heater_pin, heater_current);//ERIK + } +} + +*/ + + +// NOT USED + + diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h b/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h new file mode 100644 index 00000000..3268bc24 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h @@ -0,0 +1,148 @@ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +// Comment the next line out to use the Arduino +#define SANGUINO + +// Set 1s where you have endstops; 0s where you don't +#define ENDSTOPS_MIN_ENABLED 1 +#define ENDSTOPS_MAX_ENABLED 0 + +//our command string length +#define COMMAND_SIZE 128 + +// The size of the movement buffer + +#define BUFFER_SIZE 4 + +// Number of microseconds between timer interrupts when no movement +// is happening + +#define DEFAULT_TICK 1000 + +// What delay() value to use when waiting for things to free up in milliseconds + +#define WAITING_DELAY 1 + +#define INCHES_TO_MM 25.4 + +#define MOTHERBOARD_ERIK2 +#ifdef MOTHERBOARD_ERIK2 +// define the parameters of our machine. +#define X_STEPS_PER_MM (8*8) //7.99735/// was 8 +#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM) +#define X_MOTOR_STEPS (200*8) //was: 400 half step +#define INVERT_X_DIR 0 + +#define Y_STEPS_PER_MM (8*8) //7.99735 +#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM) +#define Y_MOTOR_STEPS (200*8) // 1/8 +#define INVERT_Y_DIR 0 + +#define Z_STEPS_PER_MM 160 // 320 +#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM) +#define Z_MOTOR_STEPS 200 +#define INVERT_Z_DIR 0 + +// For when we have a stepper-driven extruder +// E_STEPS_PER_MM is the number of steps needed to +// extrude 1mm out of the nozzle. + +#define E_STEPS_PER_MM 5.0 // 5mm diameter drive - empirically adjusted +#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM) +#define E_MOTOR_STEPS 800 + +//our maximum feedrates +#define FAST_XY_FEEDRATE 3000.0 // def: 3000.0 +#define FAST_Z_FEEDRATE 90.0// def: 50.0 // was 70 + +// Data for acceleration calculations +// Comment out the next line to turn accelerations off +//#define ACCELERATION_ON +#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating + +// Set to 1 if enable pins are inverting +// For RepRap stepper boards version 1.x the enable pins are *not* inverting. +// For RepRap stepper boards version 2.x and above the enable pins are inverting. +#define INVERT_ENABLE_PINS 1 + +#if INVERT_ENABLE_PINS == 1 +#define ENABLE_ON LOW +#else +#define ENABLE_ON HIGH +#endif + +// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed) +// RepRap opto endstops are *not* inverting. +#define ENDSTOPS_INVERTING 0 + +#else +// define the parameters of our machine. +#define X_STEPS_PER_MM (8*8) //7.99735/// was 8 +#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM) +#define X_MOTOR_STEPS (200*8) //was: 400 half step +#define INVERT_X_DIR 0 + +#define Y_STEPS_PER_MM (8*8) //7.99735 +#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM) +#define Y_MOTOR_STEPS (200*8) // 1/8 +#define INVERT_Y_DIR 0 + +#define Z_STEPS_PER_MM 160 // 320 +#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM) +#define Z_MOTOR_STEPS 200 +#define INVERT_Z_DIR 0 + +// For when we have a stepper-driven extruder +// E_STEPS_PER_MM is the number of steps needed to +// extrude 1mm out of the nozzle. + +#define E_STEPS_PER_MM 5.0 // 5mm diameter drive - empirically adjusted +#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM) +#define E_MOTOR_STEPS 800 + +//our maximum feedrates +#define FAST_XY_FEEDRATE 3000.0 // def: 3000.0 +#define FAST_Z_FEEDRATE 90.0// def: 50.0 // was 70 + +// Data for acceleration calculations +// Comment out the next line to turn accelerations off +//#define ACCELERATION_ON +#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating + +// Set to 1 if enable pins are inverting +// For RepRap stepper boards version 1.x the enable pins are *not* inverting. +// For RepRap stepper boards version 2.x and above the enable pins are inverting. +#define INVERT_ENABLE_PINS 0 + +#if INVERT_ENABLE_PINS == 1 +#define ENABLE_ON LOW +#else +#define ENABLE_ON HIGH +#endif + +// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed) +// RepRap opto endstops are *not* inverting. +#define ENDSTOPS_INVERTING 0 + +#endif +// end ifndef ERIK2 +/**************************************************************************************** + * Define the PID behavior for the heater + ****************************************************************************************/ +/// Set to true to use PID for temperature. +/// false uses the old on-off mechanism. +#define TEMP_PID 1 + +// How many temperature samples to take for an average. each sample takes about 100 usecs. +#define TEMPERATURE_SAMPLES 2 + +/// The interval between heater updates, in milliseconds. +#define TEMP_UPDATE_INTERVAL 50 +#define TEMP_PID_INTEGRAL_DRIVE_MAX 110 +#define TEMP_PID_PGAIN 5.0 +#define TEMP_PID_IGAIN 0.1 +#define TEMP_PID_DGAIN 100.0 + + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h.dist b/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h.dist new file mode 100644 index 00000000..070b3f27 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/parameters.h.dist @@ -0,0 +1,77 @@ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +// Comment the next line out to use the Arduino +#define SANGUINO + +// Set 1s where you have endstops; 0s where you don't +#define ENDSTOPS_MIN_ENABLED 1 +#define ENDSTOPS_MAX_ENABLED 0 + +//our command string length +#define COMMAND_SIZE 128 + +// The size of the movement buffer + +#define BUFFER_SIZE 4 + +// Number of microseconds between timer interrupts when no movement +// is happening + +#define DEFAULT_TICK 1000 + +// What delay() value to use when waiting for things to free up in milliseconds + +#define WAITING_DELAY 1 + +#define INCHES_TO_MM 25.4 + +// define the parameters of our machine. +#define X_STEPS_PER_MM 7.99735 +#define X_STEPS_PER_INCH (X_STEPS_PER_MM*INCHES_TO_MM) +#define X_MOTOR_STEPS 400 +#define INVERT_X_DIR 0 + +#define Y_STEPS_PER_MM 7.99735 +#define Y_STEPS_PER_INCH (Y_STEPS_PER_MM*INCHES_TO_MM) +#define Y_MOTOR_STEPS 400 +#define INVERT_Y_DIR 0 + +#define Z_STEPS_PER_MM 320 +#define Z_STEPS_PER_INCH (Z_STEPS_PER_MM*INCHES_TO_MM) +#define Z_MOTOR_STEPS 400 +#define INVERT_Z_DIR 0 + +// For when we have a stepper-driven extruder +// E_STEPS_PER_MM is the number of steps needed to +// extrude 1mm out of the nozzle. + +#define E_STEPS_PER_MM 0.706 // 5mm diameter drive - empirically adjusted +#define E_STEPS_PER_INCH (E_STEPS_PER_MM*INCHES_TO_MM) +#define E_MOTOR_STEPS 400 + +//our maximum feedrates +#define FAST_XY_FEEDRATE 3000.0 +#define FAST_Z_FEEDRATE 50.0 + +// Data for acceleration calculations +// Comment out the next line to turn accelerations off +#define ACCELERATION_ON +#define SLOW_XY_FEEDRATE 1000.0 // Speed from which to start accelerating + +// Set to 1 if enable pins are inverting +// For RepRap stepper boards version 1.x the enable pins are *not* inverting. +// For RepRap stepper boards version 2.x and above the enable pins are inverting. +#define INVERT_ENABLE_PINS 1 + +#if INVERT_ENABLE_PINS == 1 +#define ENABLE_ON LOW +#else +#define ENABLE_ON HIGH +#endif + +// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed) +// RepRap opto endstops are *not* inverting. +#define ENDSTOPS_INVERTING 0 + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/pins.h b/trunk/users/erik/FiveD_GCode_Interpreter/pins.h new file mode 100644 index 00000000..cc6f8082 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/pins.h @@ -0,0 +1,84 @@ +#ifndef PINS_H +#define PINS_H + +#ifdef SANGUINO +/**************************************************************************************** +* Sanguino pin assignment +* +****************************************************************************************/ + +//cartesian bot pins +#define X_STEP_PIN (byte)15 +#define X_DIR_PIN (byte)18 +#define X_MIN_PIN (byte)20 +#define X_MAX_PIN (byte)21 +#define X_ENABLE_PIN (byte)14 + +#define Y_STEP_PIN (byte)22 +#define Y_DIR_PIN (byte)23 +#define Y_MIN_PIN (byte)25 +#define Y_MAX_PIN (byte)26 +#define Y_ENABLE_PIN (byte)14 + +#define Z_STEP_PIN (byte)29 +#define Z_DIR_PIN (byte)30 +#define Z_MIN_PIN (byte)1 +#define Z_MAX_PIN (byte)2 +#define Z_ENABLE_PIN (byte)31 + +//ERIK +#define MODIFIER_PIN (byte)3 // analoque input! + +//extruder pins +#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)19 +#define EXTRUDER_0_MOTOR_DIR_PIN (byte)10 +#define EXTRUDER_0_HEATER_PIN (byte)12 +#define EXTRUDER_0_FAN_PIN (byte)13 +#define EXTRUDER_0_TEMPERATURE_PIN (byte)4 // Analogue input +#define EXTRUDER_0_VALVE_DIR_PIN (byte)17 +#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this +#define EXTRUDER_0_STEP_ENABLE_PIN (byte)16 // 3 - Conflicts with the fan; set -ve if no stepper + +#define EXTRUDER_1_MOTOR_SPEED_PIN (byte)19 +#define EXTRUDER_1_MOTOR_DIR_PIN (byte)10 +#define EXTRUDER_1_HEATER_PIN (byte)13 +#define EXTRUDER_1_FAN_PIN (byte)13 +#define EXTRUDER_1_TEMPERATURE_PIN (byte)4 // Analogue input +#define EXTRUDER_1_VALVE_DIR_PIN (byte)17 +#define EXTRUDER_1_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this +#define EXTRUDER_1_STEP_ENABLE_PIN (byte)16 // 7 - Conflicts with the fan; set -ve if no stepper + +#else +/**************************************************************************************** +* Arduino pin assignment +* +****************************************************************************************/ + +#define X_STEP_PIN (byte)2 +#define X_DIR_PIN (byte)3 +#define X_MIN_PIN (byte)4 +#define X_MAX_PIN (byte)9 + +#define Y_STEP_PIN (byte)10 +#define Y_DIR_PIN (byte)7 +#define Y_MIN_PIN (byte)8 +#define Y_MAX_PIN (byte)13 + +#define Z_STEP_PIN (byte)19 +#define Z_DIR_PIN (byte)18 +#define Z_MIN_PIN (byte)17 +#define Z_MAX_PIN (byte)16 + + +//extruder pins +#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)11 +#define EXTRUDER_0_MOTOR_DIR_PIN (byte)12 +#define EXTRUDER_0_HEATER_PIN (byte)6 +#define EXTRUDER_0_FAN_PIN (byte)5 +#define EXTRUDER_0_TEMPERATURE_PIN (byte)0 // Analogue input +#define EXTRUDER_0_VALVE_DIR_PIN (byte)16 //NB: Conflicts with Max Z!!!! +#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)15 +#define EXTRUDER_0_STEP_ENABLE_PIN 5 // 5 - NB conflicts with the fan; set -ve if no stepper + +#endif +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/pins.h.dist b/trunk/users/erik/FiveD_GCode_Interpreter/pins.h.dist new file mode 100644 index 00000000..3581da46 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/pins.h.dist @@ -0,0 +1,81 @@ +#ifndef PINS_H +#define PINS_H + +#ifdef SANGUINO +/**************************************************************************************** +* Sanguino pin assignment +* +****************************************************************************************/ + +//cartesian bot pins +#define X_STEP_PIN (byte)15 +#define X_DIR_PIN (byte)18 +#define X_MIN_PIN (byte)20 +#define X_MAX_PIN (byte)21 +#define X_ENABLE_PIN (byte)19 + +#define Y_STEP_PIN (byte)23 +#define Y_DIR_PIN (byte)22 +#define Y_MIN_PIN (byte)25 +#define Y_MAX_PIN (byte)26 +#define Y_ENABLE_PIN (byte)19 + +#define Z_STEP_PIN (byte)29 +#define Z_DIR_PIN (byte)30 +#define Z_MIN_PIN (byte)1 +#define Z_MAX_PIN (byte)2 +#define Z_ENABLE_PIN (byte)31 + +//extruder pins +#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)12 +#define EXTRUDER_0_MOTOR_DIR_PIN (byte)16 +#define EXTRUDER_0_HEATER_PIN (byte)14 +#define EXTRUDER_0_FAN_PIN (byte)3 +#define EXTRUDER_0_TEMPERATURE_PIN (byte)4 // Analogue input +#define EXTRUDER_0_VALVE_DIR_PIN (byte)17 +#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)13 // Valve needs to be redesigned not to need this +#define EXTRUDER_0_STEP_ENABLE_PIN (byte)3 // 3 - Conflicts with the fan; set -ve if no stepper + +#define EXTRUDER_1_MOTOR_SPEED_PIN (byte)4 +#define EXTRUDER_1_MOTOR_DIR_PIN (byte)0 +#define EXTRUDER_1_HEATER_PIN (byte)24 +#define EXTRUDER_1_FAN_PIN (byte)7 +#define EXTRUDER_1_TEMPERATURE_PIN (byte)3 // Analogue input +#define EXTRUDER_1_VALVE_DIR_PIN (byte) 6 +#define EXTRUDER_1_VALVE_ENABLE_PIN (byte)5 // Valve needs to be redesigned not to need this +#define EXTRUDER_1_STEP_ENABLE_PIN (byte)-1 // 7 - Conflicts with the fan; set -ve if no stepper + +#else +/**************************************************************************************** +* Arduino pin assignment +* +****************************************************************************************/ + +#define X_STEP_PIN (byte)2 +#define X_DIR_PIN (byte)3 +#define X_MIN_PIN (byte)4 +#define X_MAX_PIN (byte)9 + +#define Y_STEP_PIN (byte)10 +#define Y_DIR_PIN (byte)7 +#define Y_MIN_PIN (byte)8 +#define Y_MAX_PIN (byte)13 + +#define Z_STEP_PIN (byte)19 +#define Z_DIR_PIN (byte)18 +#define Z_MIN_PIN (byte)17 +#define Z_MAX_PIN (byte)16 + + +//extruder pins +#define EXTRUDER_0_MOTOR_SPEED_PIN (byte)11 +#define EXTRUDER_0_MOTOR_DIR_PIN (byte)12 +#define EXTRUDER_0_HEATER_PIN (byte)6 +#define EXTRUDER_0_FAN_PIN (byte)5 +#define EXTRUDER_0_TEMPERATURE_PIN (byte)0 // Analogue input +#define EXTRUDER_0_VALVE_DIR_PIN (byte)16 //NB: Conflicts with Max Z!!!! +#define EXTRUDER_0_VALVE_ENABLE_PIN (byte)15 +#define EXTRUDER_0_STEP_ENABLE_PIN 5 // 5 - NB conflicts with the fan; set -ve if no stepper + +#endif +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/process_g_code.pde b/trunk/users/erik/FiveD_GCode_Interpreter/process_g_code.pde new file mode 100644 index 00000000..2baf7662 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/process_g_code.pde @@ -0,0 +1,539 @@ + +#include "parameters.h" +#include "pins.h" +#include "extruder.h" +#include "vectors.h" +#include "cartesian_dda.h" + +/* bit-flags for commands and parameters */ +#define GCODE_G (1<<0) +#define GCODE_M (1<<1) +#define GCODE_P (1<<2) +#define GCODE_X (1<<3) +#define GCODE_Y (1<<4) +#define GCODE_Z (1<<5) +#define GCODE_I (1<<6) +#define GCODE_J (1<<7) +#define GCODE_K (1<<8) +#define GCODE_F (1<<9) +#define GCODE_S (1<<10) +#define GCODE_Q (1<<11) +#define GCODE_R (1<<12) +#define GCODE_E (1<<13) + + +#define PARSE_INT(ch, str, len, val, seen, flag) \ + case ch: \ + len = scan_int(str, &val, &seen, flag); \ + break; + +#define PARSE_FLOAT(ch, str, len, val, seen, flag) \ + case ch: \ + len = scan_float(str, &val, &seen, flag); \ + break; + +/* gcode line parse results */ +struct GcodeParser +{ + unsigned int seen; + int G; + int M; + float P; + float X; + float Y; + float Z; + float E; + float I; + float J; + float F; + float S; + float R; + float Q; +}; + +//our command string +char cmdbuffer[COMMAND_SIZE]; +char c = '?'; +byte serial_count = 0; +boolean comment = false; + +//our feedrate variables. +//float feedrate = SLOW_XY_FEEDRATE; + +/* keep track of the last G code - this is the command mode to use + * if there is no command in the current string + */ +int last_gcode_g = -1; + +boolean abs_mode = true; //0 = incremental; 1 = absolute + +float extruder_speed = 0; + +int scan_int(char *str, int *valp); +int scan_float(char *str, float *valp); + +GcodeParser gc; /* string parse result */ + + +//init our string processing +inline void init_process_string() +{ + serial_count = 0; + comment = false; +} + +// Get a command and process it + +void get_and_do_command() +{ + //read in characters if we got them. + if (Serial.available()) + { + c = Serial.read(); + if(c == '\r') + c = '\n'; + // Throw away control chars except \n + if(c >= ' ' || c == '\n') + { + + //newlines are ends of commands. + if (c != '\n') + { + // Start of comment - ignore any bytes received from now on + if (c == ';') + comment = true; + + // If we're not in comment mode, add it to our array. + if (!comment) + cmdbuffer[serial_count++] = c; + } + + } + } + + // Data runaway? + if(serial_count >= COMMAND_SIZE) + init_process_string(); + + //if we've got a real command, do it + if (serial_count && c == '\n') + { + // Terminate string + cmdbuffer[serial_count] = 0; + + //process our command! + process_string(cmdbuffer, serial_count); + + //clear command. + init_process_string(); + + // Say we're ready for the next one + + if(debugstring[0] != 0) + { + Serial.print("ok "); + Serial.println(debugstring); + debugstring[0] = 0; + } else + Serial.println("ok"); + } +} + + + +int parse_string(struct GcodeParser * gc, char instruction[ ], int size) +{ + int ind; + int len; /* length of parameter argument */ + + gc->seen = 0; + + len=0; + /* scan the string for commands and parameters, recording the arguments for each, + * and setting the seen flag for each that is seen + */ + for (ind=0; ind<size; ind += (1+len)) + { + len = 0; + switch (instruction[ind]) + { + PARSE_INT('G', &instruction[ind+1], len, gc->G, gc->seen, GCODE_G); + PARSE_INT('M', &instruction[ind+1], len, gc->M, gc->seen, GCODE_M); + PARSE_FLOAT('S', &instruction[ind+1], len, gc->S, gc->seen, GCODE_S); + PARSE_FLOAT('P', &instruction[ind+1], len, gc->P, gc->seen, GCODE_P); + PARSE_FLOAT('X', &instruction[ind+1], len, gc->X, gc->seen, GCODE_X); + PARSE_FLOAT('Y', &instruction[ind+1], len, gc->Y, gc->seen, GCODE_Y); + PARSE_FLOAT('Z', &instruction[ind+1], len, gc->Z, gc->seen, GCODE_Z); + PARSE_FLOAT('I', &instruction[ind+1], len, gc->I, gc->seen, GCODE_I); + PARSE_FLOAT('J', &instruction[ind+1], len, gc->J, gc->seen, GCODE_J); + PARSE_FLOAT('F', &instruction[ind+1], len, gc->F, gc->seen, GCODE_F); + PARSE_FLOAT('R', &instruction[ind+1], len, gc->R, gc->seen, GCODE_R); + PARSE_FLOAT('Q', &instruction[ind+1], len, gc->Q, gc->seen, GCODE_Q); + PARSE_FLOAT('E', &instruction[ind+1], len, gc->E, gc->seen, GCODE_E); + default: + break; + } + } +} + + +//Read the string and execute instructions +void process_string(char instruction[], int size) +{ + //the character / means delete block... used for comments and stuff. + if (instruction[0] == '/') + return; + + //init baby! + FloatPoint fp; + FloatPoint sp; + float fr; + + fp.x = 0.0; + fp.y = 0.0; + fp.z = 0.0; + fp.e = 0.0; + fp.f = 0.0; + + //get all our parameters! + parse_string(&gc, instruction, size); + /* if no command was seen, but parameters were, then use the last G code as + * the current command + */ + if ((!(gc.seen & (GCODE_G | GCODE_M))) && + ((gc.seen != 0) && + (last_gcode_g >= 0)) + ) + { + /* yes - so use the previous command with the new parameters */ + gc.G = last_gcode_g; + gc.seen |= GCODE_G; + } + //did we get a gcode? + if (gc.seen & GCODE_G) + { + last_gcode_g = gc.G; /* remember this for future instructions */ + fp = where_i_am; +#ifdef MODIFIER_PIN + modifier = analogRead(MODIFIER_PIN); // Erik +if(((modifier - modifier_old) > 3) || ((modifier - modifier_old) < -3)) +{ + Serial.print("Modifier: "); + Serial.println(modifier); + modifier_old = modifier; +} +#endif + // Debug Erik: + //Serial.print("Modifier: "); + //Serial.println(modifier/512); + + if (abs_mode) + { + if (gc.seen & GCODE_X) + fp.x = gc.X; + if (gc.seen & GCODE_Y) + fp.y = gc.Y; + if (gc.seen & GCODE_Z) + fp.z = gc.Z; + if (gc.seen & GCODE_E) +#ifdef MODIFIER_PIN +if(gc.E<0) +{ + Serial.print("Reversing: "); + Serial.print(gc.E); + Serial.print(", extr pos bef: "); + Serial.print(fp.e); + fp.e += gc.E*(modifier/512); // Erik + Serial.print(", extr pos after: "); + Serial.println(fp.e); +} else +{ + fp.e += gc.E*(modifier/512); // Erik + Serial.print("fp.e is: "); + Serial.println(fp.e); +} + // Erik: Make forced incremental +#else + fp.e += gc.E; // Erik +// fp.e += gc.E; //ERIK: normaal niet aditive! + +#endif + } + else + { + if (gc.seen & GCODE_X) + fp.x += gc.X; + if (gc.seen & GCODE_Y) + fp.y += gc.Y; + if (gc.seen & GCODE_Z) + fp.z += gc.Z; + if (gc.seen & GCODE_E) +#ifndef MODIFIER_PIN + fp.e += gc.E; +#else + fp.e += gc.E*(modifier/512); // Erik +#endif + } + + // Get feedrate if supplied - feedrates are always absolute??? + if ( gc.seen & GCODE_F ) +//#ifdef MODIFIER_PIN + fp.f = gc.F*(modifier_speed/512); // ERIK +////#else +// fp.f = gc.F; +//#endif + // Process the buffered move commands first + // If we get one, return immediately + + switch (gc.G) + { + //Rapid move + case 0: + fr = fp.f; + fp.f = FAST_XY_FEEDRATE; + qMove(fp); + fp.f = fr; + return; + + // Controlled move + case 1: + qMove(fp); + return; + + //go home. + case 28: + sp.x = 0.0; + sp.y = 0.0; + sp.z = 0.0; + if(abs_mode) + sp.e = where_i_am.e; + else + sp.e = 0.0; + if(where_i_am.z != 0.0) + sp.f = FAST_Z_FEEDRATE; + else + sp.f = FAST_XY_FEEDRATE; + qMove(sp); + + return; + +/* +// We never use this... + //go home via an intermediate point. + case 30: + fr = fp.f; + if(where_i_am.z != fp.z) + fp.f = FAST_Z_FEEDRATE; + else + fp.f = FAST_XY_FEEDRATE; + fp.f = fr; + + qMove(fp); + sp.x = 0.0; + sp.y = 0.0; + sp.z = 0.0; + if(abs_mode) + sp.e = where_i_am.e; + else + sp.e = 0.0; + if(where_i_am.z != 0.0) + sp.f = FAST_Z_FEEDRATE; + else + sp.f = FAST_XY_FEEDRATE; + qMove(sp); + + return; +*/ + default: + break; + } + + // Non-buffered G commands + // Wait till the buffer q is empty first + + while(!qEmpty()) delay(WAITING_DELAY); + switch (gc.G) + { + + //Dwell + case 4: + delay((int)(gc.P + 0.5)); // Changed by AB from 1000*gc.P + break; + + //Inches for Units + case 20: + setUnits(false); + break; + + //mm for Units + case 21: + setUnits(true); + break; + + //Absolute Positioning + case 90: + abs_mode = true; + break; + + //Incremental Positioning + case 91: + abs_mode = false; + break; + + //Set position as fp + case 92: + setPosition(fp); + break; + + default: + Serial.print("huh? G"); + Serial.println(gc.G, DEC); + } + } + + + + + //find us an m code. + if (gc.seen & GCODE_M) + { + // Wait till the q is empty first + while(!qEmpty()) delay(WAITING_DELAY); + + switch (gc.M) + { + //TODO: this is a bug because search_string returns 0. gotta fix that. + case 0: + break; + /* + case 0: + //todo: stop program + break; + + case 1: + //todo: optional stop + break; + + case 2: + //todo: program end + break; + */ + +// Now, with E codes, there is no longer any idea of turning the extruder on or off. +// (But see valve on/off below.) + +/* + //turn extruder on, forward + case 101: + ex[extruder_in_use]->set_direction(1); + ex[extruder_in_use]->set_speed(extruder_speed); + break; + + //turn extruder on, reverse + case 102: + ex[extruder_in_use]->set_direction(0); + ex[extruder_in_use]->set_speed(extruder_speed); + break; + + //turn extruder off + case 103: + ex[extruder_in_use]->set_speed(0); + break; +*/ + //custom code for temperature control + case 104: + if (gc.seen & GCODE_S) + { + ex[extruder_in_use]->set_temperature((int)gc.S); + } + break; + + //custom code for temperature reading + case 105: + Serial.print("T:"); + Serial.println(ex[extruder_in_use]->get_temperature()); + break; + + //turn fan on + case 106: + ex[extruder_in_use]->set_cooler(255); + break; + + //turn fan off + case 107: + ex[extruder_in_use]->set_cooler(0); + break; +/* +// Extruder speed is now entirely controlled by E codes + //set max extruder speed, 0-255 PWM + case 108: + if (gc.seen & GCODE_S) + extruder_speed = gc.S; + break; +*/ + +// The valve (real, or virtual...) is now the way to control any extruder (such as +// a pressurised paste extruder) that cannot move using E codes. + + // Open the valve + case 126: + ex[extruder_in_use]->valve_set(true, (int)(gc.P + 0.5)); + break; + + // Close the valve + case 127: + ex[extruder_in_use]->valve_set(false, (int)(gc.P + 0.5)); + break; + + + default: + Serial.print("Huh? M"); + Serial.println(gc.M, DEC); + } + } + +} + +int scan_float(char *str, float *valp, unsigned int *seen, unsigned int flag) +{ + float res; + int len; + char *end; + + res = (float)strtod(str, &end); + + len = end - str; + + if (len > 0) + { + *valp = res; + *seen |= flag; + } + else + *valp = 0; + + return len; /* length of number */ +} + +int scan_int(char *str, int *valp, unsigned int *seen, unsigned int flag) +{ + int res; + int len; + char *end; + + res = (int)strtol(str, &end, 10); + len = end - str; + + if (len > 0) + { + *valp = res; + *seen |= flag; + } + else + *valp = 0; + + return len; /* length of number */ +} + + + diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.cpp b/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.cpp new file mode 100644 index 00000000..9a636b4d --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.cpp @@ -0,0 +1,159 @@ +#include "Timer1.h" +#include "Variables.h" +#include "Configuration.h" + +//these routines provide an easy interface for controlling timer1 interrupts + +//this handles the timer interrupt event +SIGNAL(SIG_OUTPUT_COMPARE1A) +{ + #if MOTOR_STYLE == 1 + //do encoder stuff here. + #else + //increment our index + if (motor1_dir == MC_FORWARD) + stepper_index = (stepper_index - 1) % 8; + else + stepper_index = (stepper_index + 1) % 8; + + //coil 1 + digitalWrite(MOTOR_1_DIR_PIN, coil_a_direction & (1 << stepper_index)); + if (coil_a_enabled & (1 << stepper_index)) + analogWrite(MOTOR_1_SPEED_PIN, stepper_high_pwm); + else + analogWrite(MOTOR_1_SPEED_PIN, stepper_low_pwm); + + //coil 2 + digitalWrite(MOTOR_2_DIR_PIN, coil_b_direction & (1 << stepper_index)); + if (coil_b_enabled & (1 << stepper_index)) + analogWrite(MOTOR_2_SPEED_PIN, stepper_high_pwm); + else + analogWrite(MOTOR_2_SPEED_PIN, stepper_low_pwm); + #endif +} + +void enableTimer1Interrupt() +{ + //enable our interrupt! + TIMSK1 |= (1<<OCIE1A); +} + +void disableTimer1Interrupt() +{ + TIMSK1 &= ~(1<<ICIE1); + TIMSK1 &= ~(1<<OCIE1A); +} + +void setTimer1Resolution(byte r) +{ + //from table 15-5 in that atmega168 datasheet: + // we're setting CS12 - CS10 which correspond to the binary numbers 0-5 + // 0 = no timer + // 1 = no prescaler + // 2 = clock/8 + // 3 = clock/64 + // 4 = clock/256 + // 5 = clock/1024 + + if (r > 5) + r = 5; + + TCCR1B &= B11111000; + TCCR1B |= r; +} + +void setTimer1Ceiling(unsigned int c) +{ + OCR1A = c; +} + + +unsigned int getTimer1Ceiling(unsigned long ticks) +{ + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs) + if (ticks <= 65535L) + return (ticks & 0xffff); + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs) + else if (ticks <= 524280L) + return ((ticks / 8) & 0xffff); + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs) + else if (ticks <= 4194240L) + return ((ticks / 64) & 0xffff); + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs) + else if (ticks <= 16776960L) + return (ticks / 256); + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs) + else if (ticks <= 67107840L) + return (ticks / 1024); + //its really slow... hopefully we can just get by with super slow. + else + return 65535; +} + +byte getTimer1Resolution(unsigned long ticks) +{ + // these also represent frequency: 1000000 / ticks / 2 = frequency in hz. + + // our slowest speed at our highest resolution ( (2^16-1) * 0.0625 usecs = 4095 usecs (4 millisecond max)) + // range: 8Mhz max - 122hz min + if (ticks <= 65535L) + return 1; + // our slowest speed at our next highest resolution ( (2^16-1) * 0.5 usecs = 32767 usecs (32 millisecond max)) + // range:1Mhz max - 15.26hz min + else if (ticks <= 524280L) + return 2; + // our slowest speed at our medium resolution ( (2^16-1) * 4 usecs = 262140 usecs (0.26 seconds max)) + // range: 125Khz max - 1.9hz min + else if (ticks <= 4194240L) + return 3; + // our slowest speed at our medium-low resolution ( (2^16-1) * 16 usecs = 1048560 usecs (1.04 seconds max)) + // range: 31.25Khz max - 0.475hz min + else if (ticks <= 16776960L) + return 4; + // our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max)) + // range: 7.812Khz max - 0.119hz min + else if (ticks <= 67107840L) + return 5; + //its really slow... hopefully we can just get by with super slow. + else + return 5; +} + +void setTimer1Ticks(unsigned long ticks) +{ + // ticks is the delay between interrupts in 62.5 nanosecond ticks. + // + // we break it into 5 different resolutions based on the delay. + // then we set the resolution based on the size of the delay. + // we also then calculate the timer ceiling required. (ie what the counter counts to) + // the result is the timer counts up to the appropriate time and then fires an interrupt. + + setTimer1Ceiling(getTimer1Ceiling(ticks)); + setTimer1Resolution(getTimer1Resolution(ticks)); +} + +void setupTimer1Interrupt() +{ + //clear the registers + TCCR1A = 0; + TCCR1B = 0; + TCCR1C = 0; + TIMSK1 = 0; + + //waveform generation = 0100 = CTC + TCCR1B &= ~(1<<WGM13); + TCCR1B |= (1<<WGM12); + TCCR1A &= ~(1<<WGM11); + TCCR1A &= ~(1<<WGM10); + + //output mode = 00 (disconnected) + TCCR1A &= ~(1<<COM1A1); + TCCR1A &= ~(1<<COM1A0); + TCCR1A &= ~(1<<COM1B1); + TCCR1A &= ~(1<<COM1B0); + + //start off with a slow frequency. + setTimer1Resolution(5); + setTimer1Ceiling(65535); + disableTimer1Interrupt(); +} diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.h b/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.h new file mode 100644 index 00000000..29656217 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/tmp/Timer1.h @@ -0,0 +1,9 @@ +#ifndef TIMER1_H +#define TIMER1_H + +void enableTimer1Interrupt(); +void disableTimer1Interrupt(); +void setupTimer1Interrupt(); +void setTimer1Ticks(unsigned long ticks); + +#endif // TIMER1_H diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/upload-copy b/trunk/users/erik/FiveD_GCode_Interpreter/upload-copy new file mode 100755 index 00000000..7383a7a1 --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/upload-copy @@ -0,0 +1,7 @@ +#!/bin/sh +cp cartesian_dda.h cartesian_dda.h.dist +cp extruder.h extruder.h.dist +cp parameters.h parameters.h.dist +cp pins.h pins.h.dist +cp vectors.h vectors.h.dist + diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h b/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h new file mode 100644 index 00000000..938cfe0c --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h @@ -0,0 +1,130 @@ +// Structs to hold points in 5D space. + + +#ifndef VECTORS_H +#define VECTORS_H + +#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) + +// Real-world units +struct FloatPoint +{ + float x; // Coordinate axes + float y; + float z; + float e; // Extrude length + float f; // Feedrate +}; + +inline FloatPoint operator+(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; + result.e = a.e + b.e; + result.f = a.f + b.f; + return result; +} + +inline FloatPoint operator-(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x - b.x; + result.y = a.y - b.y; + result.z = a.z - b.z; + result.e = a.e - b.e; + result.f = a.f - b.f; + return result; +} + + +// NB - the next gives neither the scalar nor the vector product + +inline FloatPoint operator*(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x * b.x; + result.y = a.y * b.y; + result.z = a.z * b.z; + result.e = a.e * b.e; + result.f = a.f * b.f; + return result; +} + +// Can't use fabs for this as it's defined somewhere in a #define + +inline FloatPoint fabsv(const FloatPoint& a) +{ + FloatPoint result; + result.x = fabs(a.x); + result.y = fabs(a.y); + result.z = fabs(a.z); + result.e = fabs(a.e); + result.f = fabs(a.f); + return result; +} + + +// Integer numbers of steps +struct LongPoint +{ + long x; // Coordinates + long y; + long z; + long e; // Extrusion + long f; // Feedrate +}; + +inline LongPoint operator+(const LongPoint& a, const LongPoint& b) +{ + LongPoint result; + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; + result.e = a.e + b.e; + result.f = a.f + b.f; + return result; +} + +inline LongPoint operator-(const LongPoint& a, const LongPoint& b) +{ + LongPoint result; + result.x = a.x - b.x; + result.y = a.y - b.y; + result.z = a.z - b.z; + result.e = a.e - b.e; + result.f = a.f - b.f; + return result; +} + + +inline LongPoint absv(const LongPoint& a) +{ + LongPoint result; + result.x = abs(a.x); + result.y = abs(a.y); + result.z = abs(a.z); + result.e = abs(a.e); + result.f = abs(a.f); + return result; +} + + +inline LongPoint roundv(const FloatPoint& a) +{ + LongPoint result; + result.x = round(a.x); + result.y = round(a.y); + result.z = round(a.z); + result.e = round(a.e); + result.f = round(a.f); + return result; +} + +inline LongPoint to_steps(const FloatPoint& units, const FloatPoint& position) +{ + return roundv(units*position); +} + +#endif diff --git a/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h.dist b/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h.dist new file mode 100644 index 00000000..938cfe0c --- /dev/null +++ b/trunk/users/erik/FiveD_GCode_Interpreter/vectors.h.dist @@ -0,0 +1,130 @@ +// Structs to hold points in 5D space. + + +#ifndef VECTORS_H +#define VECTORS_H + +#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) + +// Real-world units +struct FloatPoint +{ + float x; // Coordinate axes + float y; + float z; + float e; // Extrude length + float f; // Feedrate +}; + +inline FloatPoint operator+(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; + result.e = a.e + b.e; + result.f = a.f + b.f; + return result; +} + +inline FloatPoint operator-(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x - b.x; + result.y = a.y - b.y; + result.z = a.z - b.z; + result.e = a.e - b.e; + result.f = a.f - b.f; + return result; +} + + +// NB - the next gives neither the scalar nor the vector product + +inline FloatPoint operator*(const FloatPoint& a, const FloatPoint& b) +{ + FloatPoint result; + result.x = a.x * b.x; + result.y = a.y * b.y; + result.z = a.z * b.z; + result.e = a.e * b.e; + result.f = a.f * b.f; + return result; +} + +// Can't use fabs for this as it's defined somewhere in a #define + +inline FloatPoint fabsv(const FloatPoint& a) +{ + FloatPoint result; + result.x = fabs(a.x); + result.y = fabs(a.y); + result.z = fabs(a.z); + result.e = fabs(a.e); + result.f = fabs(a.f); + return result; +} + + +// Integer numbers of steps +struct LongPoint +{ + long x; // Coordinates + long y; + long z; + long e; // Extrusion + long f; // Feedrate +}; + +inline LongPoint operator+(const LongPoint& a, const LongPoint& b) +{ + LongPoint result; + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; + result.e = a.e + b.e; + result.f = a.f + b.f; + return result; +} + +inline LongPoint operator-(const LongPoint& a, const LongPoint& b) +{ + LongPoint result; + result.x = a.x - b.x; + result.y = a.y - b.y; + result.z = a.z - b.z; + result.e = a.e - b.e; + result.f = a.f - b.f; + return result; +} + + +inline LongPoint absv(const LongPoint& a) +{ + LongPoint result; + result.x = abs(a.x); + result.y = abs(a.y); + result.z = abs(a.z); + result.e = abs(a.e); + result.f = abs(a.f); + return result; +} + + +inline LongPoint roundv(const FloatPoint& a) +{ + LongPoint result; + result.x = round(a.x); + result.y = round(a.y); + result.z = round(a.z); + result.e = round(a.e); + result.f = round(a.f); + return result; +} + +inline LongPoint to_steps(const FloatPoint& units, const FloatPoint& position) +{ + return roundv(units*position); +} + +#endif |