Merged Marlin, Marlin non gen6 and Ultimaker changes
Erik van der Zalm
13 years ago
parent
0b1423c303
commit
094afe7c10
@ -0,0 +1,123 @@
|
||||
|
||||
#include "planner.h"
|
||||
#include "temperature.h"
|
||||
|
||||
//======================================================================================
|
||||
template <class T> int EEPROM_writeAnything(int &ee, const T& value)
|
||||
{
|
||||
const byte* p = (const byte*)(const void*)&value;
|
||||
int i;
|
||||
for (i = 0; i < sizeof(value); i++)
|
||||
EEPROM.write(ee++, *p++);
|
||||
return i;
|
||||
}
|
||||
//======================================================================================
|
||||
template <class T> int EEPROM_readAnything(int &ee, T& value)
|
||||
{
|
||||
byte* p = (byte*)(void*)&value;
|
||||
int i;
|
||||
for (i = 0; i < sizeof(value); i++)
|
||||
*p++ = EEPROM.read(ee++);
|
||||
return i;
|
||||
}
|
||||
//======================================================================================
|
||||
|
||||
#define EEPROM_OFFSET 100
|
||||
|
||||
#define EEPROM_VERSION "V04" // IMPORTANT: Whenever there are changes made to the variables stored in EEPROM
|
||||
// in the functions below, also increment the version number. This makes sure that
|
||||
// the default values are used whenever there is a change to the data, to prevent
|
||||
// wrong data being written to the variables.
|
||||
// ALSO: always make sure the variables in the Store and retrieve sections are in the same order.
|
||||
void StoreSettings() {
|
||||
char ver[4]= "000";
|
||||
int i=EEPROM_OFFSET;
|
||||
EEPROM_writeAnything(i,ver); // invalidate data first
|
||||
EEPROM_writeAnything(i,axis_steps_per_unit);
|
||||
EEPROM_writeAnything(i,max_feedrate);
|
||||
EEPROM_writeAnything(i,max_acceleration_units_per_sq_second);
|
||||
EEPROM_writeAnything(i,acceleration);
|
||||
EEPROM_writeAnything(i,retract_acceleration);
|
||||
EEPROM_writeAnything(i,minimumfeedrate);
|
||||
EEPROM_writeAnything(i,mintravelfeedrate);
|
||||
EEPROM_writeAnything(i,minsegmenttime);
|
||||
EEPROM_writeAnything(i,max_xy_jerk);
|
||||
EEPROM_writeAnything(i,max_z_jerk);
|
||||
#ifdef PIDTEMP
|
||||
EEPROM_writeAnything(i,Kp);
|
||||
EEPROM_writeAnything(i,Ki);
|
||||
EEPROM_writeAnything(i,Kd);
|
||||
#else
|
||||
EEPROM_writeAnything(i,3000);
|
||||
EEPROM_writeAnything(i,0);
|
||||
EEPROM_writeAnything(i,0);
|
||||
#endif
|
||||
char ver2[4]=EEPROM_VERSION;
|
||||
i=EEPROM_OFFSET;
|
||||
EEPROM_writeAnything(i,ver2); // validate data
|
||||
ECHOLN("Settings Stored");
|
||||
|
||||
}
|
||||
|
||||
void RetrieveSettings(bool def=false){ // if def=true, the default values will be used
|
||||
int i=EEPROM_OFFSET;
|
||||
char stored_ver[4];
|
||||
char ver[4]=EEPROM_VERSION;
|
||||
EEPROM_readAnything(i,stored_ver); //read stored version
|
||||
// ECHOLN("Version: [" << ver << "] Stored version: [" << stored_ver << "]");
|
||||
if ((!def)&&(strncmp(ver,stored_ver,3)==0)) { // version number match
|
||||
EEPROM_readAnything(i,axis_steps_per_unit);
|
||||
EEPROM_readAnything(i,max_feedrate);
|
||||
EEPROM_readAnything(i,max_acceleration_units_per_sq_second);
|
||||
EEPROM_readAnything(i,acceleration);
|
||||
EEPROM_readAnything(i,retract_acceleration);
|
||||
EEPROM_readAnything(i,minimumfeedrate);
|
||||
EEPROM_readAnything(i,mintravelfeedrate);
|
||||
EEPROM_readAnything(i,minsegmenttime);
|
||||
EEPROM_readAnything(i,max_xy_jerk);
|
||||
EEPROM_readAnything(i,max_z_jerk);
|
||||
#ifndef PIDTEMP
|
||||
float Kp,Ki,Kd;
|
||||
#endif
|
||||
EEPROM_readAnything(i,Kp);
|
||||
EEPROM_readAnything(i,Ki);
|
||||
EEPROM_readAnything(i,Kd);
|
||||
|
||||
ECHOLN("Stored settings retreived:");
|
||||
}
|
||||
else {
|
||||
float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
|
||||
float tmp2[]=DEFAULT_MAX_FEEDRATE;
|
||||
long tmp3[]=DEFAULT_MAX_ACCELERATION;
|
||||
for (int i=0;i<4;i++) {
|
||||
axis_steps_per_unit[i]=tmp1[i];
|
||||
max_feedrate[i]=tmp2[i];
|
||||
max_acceleration_units_per_sq_second[i]=tmp3[i];
|
||||
}
|
||||
acceleration=DEFAULT_ACCELERATION;
|
||||
retract_acceleration=DEFAULT_RETRACT_ACCELERATION;
|
||||
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
|
||||
minsegmenttime=DEFAULT_MINSEGMENTTIME;
|
||||
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
|
||||
max_xy_jerk=DEFAULT_XYJERK;
|
||||
max_z_jerk=DEFAULT_ZJERK;
|
||||
ECHOLN("Using Default settings:");
|
||||
}
|
||||
ECHOLN("Steps per unit:");
|
||||
ECHOLN(" M92 X" <<_FLOAT(axis_steps_per_unit[0],3) << " Y" << _FLOAT(axis_steps_per_unit[1],3) << " Z" << _FLOAT(axis_steps_per_unit[2],3) << " E" << _FLOAT(axis_steps_per_unit[3],3));
|
||||
ECHOLN("Maximum feedrates (mm/s):");
|
||||
ECHOLN(" M203 X" <<_FLOAT(max_feedrate[0]/60,2)<<" Y" << _FLOAT(max_feedrate[1]/60,2) << " Z" << _FLOAT(max_feedrate[2]/60,2) << " E" << _FLOAT(max_feedrate[3]/60,2));
|
||||
ECHOLN("Maximum Acceleration (mm/s2):");
|
||||
ECHOLN(" M201 X" <<_FLOAT(max_acceleration_units_per_sq_second[0],0) << " Y" << _FLOAT(max_acceleration_units_per_sq_second[1],0) << " Z" << _FLOAT(max_acceleration_units_per_sq_second[2],0) << " E" << _FLOAT(max_acceleration_units_per_sq_second[3],0));
|
||||
ECHOLN("Acceleration: S=acceleration, T=retract acceleration");
|
||||
ECHOLN(" M204 S" <<_FLOAT(acceleration,2) << " T" << _FLOAT(retract_acceleration,2));
|
||||
ECHOLN("Advanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum xY jerk (mm/s), Z=maximum Z jerk (mm/s)");
|
||||
ECHOLN(" M205 S" <<_FLOAT(minimumfeedrate/60,2) << " T" << _FLOAT(mintravelfeedrate/60,2) << " B" << _FLOAT(minsegmenttime,2) << " X" << _FLOAT(max_xy_jerk/60,2) << " Z" << _FLOAT(max_z_jerk/60,2));
|
||||
#ifdef PIDTEMP
|
||||
ECHOLN("PID settings:");
|
||||
ECHOLN(" M301 P" << _FLOAT(Kp,3) << " I" << _FLOAT(Ki,3) << " D" << _FLOAT(Kd,3));
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,10 @@
|
||||
#ifndef __LCDH
|
||||
#define __LCDH
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -0,0 +1 @@
|
||||
|
||||
@ -0,0 +1,584 @@
|
||||
/*
|
||||
planner.c - buffers movement commands and manages the acceleration profile plan
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
|
||||
|
||||
/*
|
||||
Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
|
||||
|
||||
s == speed, a == acceleration, t == time, d == distance
|
||||
|
||||
Basic definitions:
|
||||
|
||||
Speed[s_, a_, t_] := s + (a*t)
|
||||
Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
|
||||
|
||||
Distance to reach a specific speed with a constant acceleration:
|
||||
|
||||
Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
|
||||
d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
|
||||
|
||||
Speed after a given distance of travel with constant acceleration:
|
||||
|
||||
Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
|
||||
m -> Sqrt[2 a d + s^2]
|
||||
|
||||
DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
|
||||
|
||||
When to start braking (di) to reach a specified destionation speed (s2) after accelerating
|
||||
from initial speed s1 without ever stopping at a plateau:
|
||||
|
||||
Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
|
||||
di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
|
||||
|
||||
IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
|
||||
*/
|
||||
|
||||
|
||||
//#include <inttypes.h>
|
||||
//#include <math.h>
|
||||
//#include <stdlib.h>
|
||||
|
||||
#include "Marlin.h"
|
||||
#include "Configuration.h"
|
||||
#include "pins.h"
|
||||
#include "fastio.h"
|
||||
#include "planner.h"
|
||||
#include "stepper.h"
|
||||
#include "temperature.h"
|
||||
#include "ultralcd.h"
|
||||
|
||||
unsigned long minsegmenttime;
|
||||
float max_feedrate[4]; // set the max speeds
|
||||
float axis_steps_per_unit[4];
|
||||
long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software
|
||||
float minimumfeedrate;
|
||||
float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
|
||||
float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX
|
||||
float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
|
||||
float max_z_jerk;
|
||||
float mintravelfeedrate;
|
||||
unsigned long axis_steps_per_sqr_second[NUM_AXIS];
|
||||
// Manage heater variables.
|
||||
|
||||
static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions
|
||||
static volatile unsigned char block_buffer_head; // Index of the next block to be pushed
|
||||
static volatile unsigned char block_buffer_tail; // Index of the block to process now
|
||||
|
||||
// The current position of the tool in absolute steps
|
||||
long position[4];
|
||||
|
||||
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
|
||||
|
||||
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
|
||||
// given acceleration:
|
||||
inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
|
||||
if (acceleration!=0) {
|
||||
return((target_rate*target_rate-initial_rate*initial_rate)/
|
||||
(2.0*acceleration));
|
||||
}
|
||||
else {
|
||||
return 0.0; // acceleration was 0, set acceleration distance to 0
|
||||
}
|
||||
}
|
||||
|
||||
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
|
||||
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
|
||||
// a total travel of distance. This can be used to compute the intersection point between acceleration and
|
||||
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
|
||||
|
||||
inline float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
|
||||
if (acceleration!=0) {
|
||||
return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
|
||||
(4.0*acceleration) );
|
||||
}
|
||||
else {
|
||||
return 0.0; // acceleration was 0, set intersection distance to 0
|
||||
}
|
||||
}
|
||||
|
||||
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
|
||||
|
||||
void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) {
|
||||
if(block->busy == true) return; // If block is busy then bail out.
|
||||
float entry_factor = entry_speed / block->nominal_speed;
|
||||
float exit_factor = exit_speed / block->nominal_speed;
|
||||
long initial_rate = ceil(block->nominal_rate*entry_factor);
|
||||
long final_rate = ceil(block->nominal_rate*exit_factor);
|
||||
|
||||
#ifdef ADVANCE
|
||||
long initial_advance = block->advance*entry_factor*entry_factor;
|
||||
long final_advance = block->advance*exit_factor*exit_factor;
|
||||
#endif // ADVANCE
|
||||
|
||||
// Limit minimal step rate (Otherwise the timer will overflow.)
|
||||
if(initial_rate <120) initial_rate=120;
|
||||
if(final_rate < 120) final_rate=120;
|
||||
|
||||
// Calculate the acceleration steps
|
||||
long acceleration = block->acceleration_st;
|
||||
long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration);
|
||||
long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration);
|
||||
// Calculate the size of Plateau of Nominal Rate.
|
||||
long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
|
||||
|
||||
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
|
||||
// have to use intersection_distance() to calculate when to abort acceleration and start braking
|
||||
// in order to reach the final_rate exactly at the end of this block.
|
||||
if (plateau_steps < 0) {
|
||||
accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count);
|
||||
plateau_steps = 0;
|
||||
}
|
||||
|
||||
long decelerate_after = accelerate_steps+plateau_steps;
|
||||
|
||||
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
|
||||
if(block->busy == false) { // Don't update variables if block is busy.
|
||||
block->accelerate_until = accelerate_steps;
|
||||
block->decelerate_after = decelerate_after;
|
||||
block->initial_rate = initial_rate;
|
||||
block->final_rate = final_rate;
|
||||
#ifdef ADVANCE
|
||||
block->initial_advance = initial_advance;
|
||||
block->final_advance = final_advance;
|
||||
#endif //ADVANCE
|
||||
}
|
||||
CRITICAL_SECTION_END;
|
||||
}
|
||||
|
||||
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
|
||||
// acceleration within the allotted distance.
|
||||
inline float max_allowable_speed(float acceleration, float target_velocity, float distance) {
|
||||
return(
|
||||
sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance)
|
||||
);
|
||||
}
|
||||
|
||||
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
|
||||
// This method will calculate the junction jerk as the euclidean distance between the nominal
|
||||
// velocities of the respective blocks.
|
||||
inline float junction_jerk(block_t *before, block_t *after) {
|
||||
return(sqrt(
|
||||
pow((before->speed_x-after->speed_x), 2)+
|
||||
pow((before->speed_y-after->speed_y), 2)));
|
||||
}
|
||||
|
||||
// Return the safe speed which is max_jerk/2, e.g. the
|
||||
// speed under which you cannot exceed max_jerk no matter what you do.
|
||||
float safe_speed(block_t *block) {
|
||||
float safe_speed;
|
||||
safe_speed = max_xy_jerk/2;
|
||||
if(abs(block->speed_z) > max_z_jerk/2) safe_speed = max_z_jerk/2;
|
||||
if (safe_speed > block->nominal_speed) safe_speed = block->nominal_speed;
|
||||
return safe_speed;
|
||||
}
|
||||
|
||||
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
|
||||
void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
|
||||
if(!current) {
|
||||
return;
|
||||
}
|
||||
|
||||
float entry_speed = current->nominal_speed;
|
||||
float exit_factor;
|
||||
float exit_speed;
|
||||
if (next) {
|
||||
exit_speed = next->entry_speed;
|
||||
}
|
||||
else {
|
||||
exit_speed = safe_speed(current);
|
||||
}
|
||||
|
||||
// Calculate the entry_factor for the current block.
|
||||
if (previous) {
|
||||
// Reduce speed so that junction_jerk is within the maximum allowed
|
||||
float jerk = junction_jerk(previous, current);
|
||||
if((previous->steps_x == 0) && (previous->steps_y == 0)) {
|
||||
entry_speed = safe_speed(current);
|
||||
}
|
||||
else if (jerk > max_xy_jerk) {
|
||||
entry_speed = (max_xy_jerk/jerk) * entry_speed;
|
||||
}
|
||||
if(abs(previous->speed_z - current->speed_z) > max_z_jerk) {
|
||||
entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed;
|
||||
}
|
||||
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
|
||||
if (entry_speed > exit_speed) {
|
||||
float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters);
|
||||
if (max_entry_speed < entry_speed) {
|
||||
entry_speed = max_entry_speed;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
entry_speed = safe_speed(current);
|
||||
}
|
||||
// Store result
|
||||
current->entry_speed = entry_speed;
|
||||
}
|
||||
|
||||
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
|
||||
// implements the reverse pass.
|
||||
void planner_reverse_pass() {
|
||||
char block_index = block_buffer_head;
|
||||
if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
|
||||
block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
|
||||
block_t *block[5] = {
|
||||
NULL, NULL, NULL, NULL, NULL };
|
||||
while(block_index != block_buffer_tail) {
|
||||
block_index = (block_index-1) & (BLOCK_BUFFER_SIZE -1);
|
||||
block[2]= block[1];
|
||||
block[1]= block[0];
|
||||
block[0] = &block_buffer[block_index];
|
||||
planner_reverse_pass_kernel(block[0], block[1], block[2]);
|
||||
}
|
||||
planner_reverse_pass_kernel(NULL, block[0], block[1]);
|
||||
}
|
||||
}
|
||||
|
||||
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
|
||||
void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
|
||||
if(!current) {
|
||||
return;
|
||||
}
|
||||
if(previous) {
|
||||
// If the previous block is an acceleration block, but it is not long enough to
|
||||
// complete the full speed change within the block, we need to adjust out entry
|
||||
// speed accordingly. Remember current->entry_factor equals the exit factor of
|
||||
// the previous block.
|
||||
if(previous->entry_speed < current->entry_speed) {
|
||||
float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);
|
||||
if (max_entry_speed < current->entry_speed) {
|
||||
current->entry_speed = max_entry_speed;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
|
||||
// implements the forward pass.
|
||||
void planner_forward_pass() {
|
||||
char block_index = block_buffer_tail;
|
||||
block_t *block[3] = {
|
||||
NULL, NULL, NULL };
|
||||
|
||||
while(block_index != block_buffer_head) {
|
||||
block[0] = block[1];
|
||||
block[1] = block[2];
|
||||
block[2] = &block_buffer[block_index];
|
||||
planner_forward_pass_kernel(block[0],block[1],block[2]);
|
||||
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
|
||||
}
|
||||
planner_forward_pass_kernel(block[1], block[2], NULL);
|
||||
}
|
||||
|
||||
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
|
||||
// entry_factor for each junction. Must be called by planner_recalculate() after
|
||||
// updating the blocks.
|
||||
void planner_recalculate_trapezoids() {
|
||||
char block_index = block_buffer_tail;
|
||||
block_t *current;
|
||||
block_t *next = NULL;
|
||||
while(block_index != block_buffer_head) {
|
||||
current = next;
|
||||
next = &block_buffer[block_index];
|
||||
if (current) {
|
||||
calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed);
|
||||
}
|
||||
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
|
||||
}
|
||||
calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next));
|
||||
}
|
||||
|
||||
// Recalculates the motion plan according to the following algorithm:
|
||||
//
|
||||
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
|
||||
// so that:
|
||||
// a. The junction jerk is within the set limit
|
||||
// b. No speed reduction within one block requires faster deceleration than the one, true constant
|
||||
// acceleration.
|
||||
// 2. Go over every block in chronological order and dial down junction speed reduction values if
|
||||
// a. The speed increase within one block would require faster accelleration than the one, true
|
||||
// constant acceleration.
|
||||
//
|
||||
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
|
||||
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
|
||||
// the set limit. Finally it will:
|
||||
//
|
||||
// 3. Recalculate trapezoids for all blocks.
|
||||
|
||||
void planner_recalculate() {
|
||||
planner_reverse_pass();
|
||||
planner_forward_pass();
|
||||
planner_recalculate_trapezoids();
|
||||
}
|
||||
|
||||
void plan_init() {
|
||||
block_buffer_head = 0;
|
||||
block_buffer_tail = 0;
|
||||
memset(position, 0, sizeof(position)); // clear position
|
||||
}
|
||||
|
||||
|
||||
void plan_discard_current_block() {
|
||||
if (block_buffer_head != block_buffer_tail) {
|
||||
block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);
|
||||
}
|
||||
}
|
||||
|
||||
block_t *plan_get_current_block() {
|
||||
if (block_buffer_head == block_buffer_tail) {
|
||||
return(NULL);
|
||||
}
|
||||
block_t *block = &block_buffer[block_buffer_tail];
|
||||
block->busy = true;
|
||||
return(block);
|
||||
}
|
||||
|
||||
void check_axes_activity() {
|
||||
unsigned char x_active = 0;
|
||||
unsigned char y_active = 0;
|
||||
unsigned char z_active = 0;
|
||||
unsigned char e_active = 0;
|
||||
block_t *block;
|
||||
|
||||
if(block_buffer_tail != block_buffer_head) {
|
||||
char block_index = block_buffer_tail;
|
||||
while(block_index != block_buffer_head) {
|
||||
block = &block_buffer[block_index];
|
||||
if(block->steps_x != 0) x_active++;
|
||||
if(block->steps_y != 0) y_active++;
|
||||
if(block->steps_z != 0) z_active++;
|
||||
if(block->steps_e != 0) e_active++;
|
||||
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
|
||||
}
|
||||
}
|
||||
if((DISABLE_X) && (x_active == 0)) disable_x();
|
||||
if((DISABLE_Y) && (y_active == 0)) disable_y();
|
||||
if((DISABLE_Z) && (z_active == 0)) disable_z();
|
||||
if((DISABLE_E) && (e_active == 0)) disable_e();
|
||||
}
|
||||
|
||||
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
|
||||
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
|
||||
// calculation the caller must also provide the physical length of the line in millimeters.
|
||||
void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
||||
|
||||
// The target position of the tool in absolute steps
|
||||
// Calculate target position in absolute steps
|
||||
long target[4];
|
||||
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
|
||||
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
||||
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
||||
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
||||
|
||||
// Calculate the buffer head after we push this byte
|
||||
int next_buffer_head = (block_buffer_head + 1) & (BLOCK_BUFFER_SIZE - 1);
|
||||
|
||||
// If the buffer is full: good! That means we are well ahead of the robot.
|
||||
// Rest here until there is room in the buffer.
|
||||
while(block_buffer_tail == next_buffer_head) {
|
||||
manage_heater();
|
||||
manage_inactivity(1);
|
||||
LCD_STATUS;
|
||||
}
|
||||
|
||||
// Prepare to set up new block
|
||||
block_t *block = &block_buffer[block_buffer_head];
|
||||
|
||||
// Mark block as not busy (Not executed by the stepper interrupt)
|
||||
block->busy = false;
|
||||
|
||||
// Number of steps for each axis
|
||||
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
|
||||
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
|
||||
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
|
||||
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
|
||||
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
|
||||
|
||||
// Bail if this is a zero-length block
|
||||
if (block->step_event_count <=dropsegments) {
|
||||
return;
|
||||
};
|
||||
|
||||
//enable active axes
|
||||
if(block->steps_x != 0) enable_x();
|
||||
if(block->steps_y != 0) enable_y();
|
||||
if(block->steps_z != 0) enable_z();
|
||||
if(block->steps_e != 0) enable_e();
|
||||
|
||||
float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
|
||||
float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
||||
float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
|
||||
float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
|
||||
block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm));
|
||||
|
||||
unsigned long microseconds;
|
||||
|
||||
if (block->steps_e == 0) {
|
||||
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
|
||||
}
|
||||
else {
|
||||
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
|
||||
}
|
||||
|
||||
microseconds = lround((block->millimeters/feed_rate)*1000000);
|
||||
|
||||
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
|
||||
// reduces/removes corner blobs as the machine won't come to a full stop.
|
||||
int blockcount=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
|
||||
|
||||
if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) {
|
||||
if (microseconds<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
||||
microseconds=microseconds+lround(2*(minsegmenttime-microseconds)/blockcount);
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (microseconds<minsegmenttime) microseconds=minsegmenttime;
|
||||
}
|
||||
// END OF SLOW DOWN SECTION
|
||||
|
||||
|
||||
// Calculate speed in mm/minute for each axis
|
||||
float multiplier = 60.0*1000000.0/microseconds;
|
||||
block->speed_z = delta_z_mm * multiplier;
|
||||
block->speed_x = delta_x_mm * multiplier;
|
||||
block->speed_y = delta_y_mm * multiplier;
|
||||
block->speed_e = delta_e_mm * multiplier;
|
||||
|
||||
|
||||
// Limit speed per axis
|
||||
float speed_factor = 1; //factor <=1 do decrease speed
|
||||
if(abs(block->speed_x) > max_feedrate[X_AXIS]) {
|
||||
//// [ErikDeBruijn] IS THIS THE BUG WE'RE LOOING FOR????
|
||||
//// [bernhard] No its not, according to Zalm.
|
||||
//// the if would always be true, since tmp_speedfactor <=0 due the inial if, so its safe to set. the next lines actually compare.
|
||||
speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x);
|
||||
//if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
|
||||
}
|
||||
if(abs(block->speed_y) > max_feedrate[Y_AXIS]){
|
||||
float tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y);
|
||||
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
|
||||
}
|
||||
if(abs(block->speed_z) > max_feedrate[Z_AXIS]){
|
||||
float tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z);
|
||||
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
|
||||
}
|
||||
if(abs(block->speed_e) > max_feedrate[E_AXIS]){
|
||||
float tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e);
|
||||
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
|
||||
}
|
||||
multiplier = multiplier * speed_factor;
|
||||
block->speed_z = delta_z_mm * multiplier;
|
||||
block->speed_x = delta_x_mm * multiplier;
|
||||
block->speed_y = delta_y_mm * multiplier;
|
||||
block->speed_e = delta_e_mm * multiplier;
|
||||
block->nominal_speed = block->millimeters * multiplier;
|
||||
block->nominal_rate = ceil(block->step_event_count * multiplier / 60);
|
||||
|
||||
if(block->nominal_rate < 120) block->nominal_rate = 120;
|
||||
block->entry_speed = safe_speed(block);
|
||||
|
||||
// Compute the acceleration rate for the trapezoid generator.
|
||||
float travel_per_step = block->millimeters/block->step_event_count;
|
||||
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
|
||||
block->acceleration_st = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
|
||||
}
|
||||
else {
|
||||
block->acceleration_st = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
|
||||
float tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;
|
||||
// Limit acceleration per axis
|
||||
if((tmp_acceleration * block->steps_x) > axis_steps_per_sqr_second[X_AXIS]) {
|
||||
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
|
||||
tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;
|
||||
}
|
||||
if((tmp_acceleration * block->steps_y) > axis_steps_per_sqr_second[Y_AXIS]) {
|
||||
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
|
||||
tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;
|
||||
}
|
||||
if((tmp_acceleration * block->steps_e) > axis_steps_per_sqr_second[E_AXIS]) {
|
||||
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
|
||||
tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;
|
||||
}
|
||||
if((tmp_acceleration * block->steps_z) > axis_steps_per_sqr_second[Z_AXIS]) {
|
||||
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
|
||||
tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count;
|
||||
}
|
||||
}
|
||||
block->acceleration = block->acceleration_st * travel_per_step;
|
||||
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
|
||||
|
||||
#ifdef ADVANCE
|
||||
// Calculate advance rate
|
||||
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
|
||||
block->advance_rate = 0;
|
||||
block->advance = 0;
|
||||
}
|
||||
else {
|
||||
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
|
||||
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
|
||||
(block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;
|
||||
block->advance = advance;
|
||||
if(acc_dist == 0) {
|
||||
block->advance_rate = 0;
|
||||
}
|
||||
else {
|
||||
block->advance_rate = advance / (float)acc_dist;
|
||||
}
|
||||
}
|
||||
#endif // ADVANCE
|
||||
|
||||
// compute a preliminary conservative acceleration trapezoid
|
||||
float safespeed = safe_speed(block);
|
||||
calculate_trapezoid_for_block(block, safespeed, safespeed);
|
||||
|
||||
// Compute direction bits for this block
|
||||
block->direction_bits = 0;
|
||||
if (target[X_AXIS] < position[X_AXIS]) {
|
||||
block->direction_bits |= (1<<X_AXIS);
|
||||
}
|
||||
if (target[Y_AXIS] < position[Y_AXIS]) {
|
||||
block->direction_bits |= (1<<Y_AXIS);
|
||||
}
|
||||
if (target[Z_AXIS] < position[Z_AXIS]) {
|
||||
block->direction_bits |= (1<<Z_AXIS);
|
||||
}
|
||||
if (target[E_AXIS] < position[E_AXIS]) {
|
||||
block->direction_bits |= (1<<E_AXIS);
|
||||
}
|
||||
|
||||
// Move buffer head
|
||||
block_buffer_head = next_buffer_head;
|
||||
|
||||
// Update position
|
||||
memcpy(position, target, sizeof(target)); // position[] = target[]
|
||||
|
||||
planner_recalculate();
|
||||
st_wake_up();
|
||||
}
|
||||
|
||||
void plan_set_position(float x, float y, float z, float e)
|
||||
{
|
||||
position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
|
||||
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
||||
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
||||
position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
||||
}
|
||||
|
||||
@ -0,0 +1,90 @@
|
||||
/*
|
||||
planner.h - buffers movement commands and manages the acceleration profile plan
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// This module is to be considered a sub-module of stepper.c. Please don't include
|
||||
// this file from any other module.
|
||||
|
||||
#ifndef planner_h
|
||||
#define planner_h
|
||||
|
||||
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
|
||||
// the source g-code and may never actually be reached if acceleration management is active.
|
||||
typedef struct {
|
||||
// Fields used by the bresenham algorithm for tracing the line
|
||||
long steps_x, steps_y, steps_z, steps_e; // Step count along each axis
|
||||
long step_event_count; // The number of step events required to complete this block
|
||||
volatile long accelerate_until; // The index of the step event on which to stop acceleration
|
||||
volatile long decelerate_after; // The index of the step event on which to start decelerating
|
||||
volatile long acceleration_rate; // The acceleration rate used for acceleration calculation
|
||||
unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
|
||||
#ifdef ADVANCE
|
||||
long advance_rate;
|
||||
volatile long initial_advance;
|
||||
volatile long final_advance;
|
||||
float advance;
|
||||
#endif
|
||||
|
||||
// Fields used by the motion planner to manage acceleration
|
||||
float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis
|
||||
float nominal_speed; // The nominal speed for this block in mm/min
|
||||
float millimeters; // The total travel of this block in mm
|
||||
float entry_speed;
|
||||
float acceleration; // acceleration mm/sec^2
|
||||
|
||||
// Settings for the trapezoid generator
|
||||
long nominal_rate; // The nominal step rate for this block in step_events/sec
|
||||
volatile long initial_rate; // The jerk-adjusted step rate at start of block
|
||||
volatile long final_rate; // The minimal rate at exit
|
||||
long acceleration_st; // acceleration steps/sec^2
|
||||
volatile char busy;
|
||||
} block_t;
|
||||
|
||||
// Initialize the motion plan subsystem
|
||||
void plan_init();
|
||||
|
||||
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
|
||||
// millimaters. Feed rate specifies the speed of the motion.
|
||||
void plan_buffer_line(float x, float y, float z, float e, float feed_rate);
|
||||
|
||||
// Set position. Used for G92 instructions.
|
||||
void plan_set_position(float x, float y, float z, float e);
|
||||
|
||||
// Called when the current block is no longer needed. Discards the block and makes the memory
|
||||
// availible for new blocks.
|
||||
void plan_discard_current_block();
|
||||
|
||||
// Gets the current block. Returns NULL if buffer empty
|
||||
block_t *plan_get_current_block();
|
||||
|
||||
void check_axes_activity();
|
||||
|
||||
extern unsigned long minsegmenttime;
|
||||
extern float max_feedrate[4]; // set the max speeds
|
||||
extern float axis_steps_per_unit[4];
|
||||
extern long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software
|
||||
extern float minimumfeedrate;
|
||||
extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
|
||||
extern float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX
|
||||
extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
|
||||
extern float max_z_jerk;
|
||||
extern float mintravelfeedrate;
|
||||
extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,592 @@
|
||||
/*
|
||||
stepper.c - stepper motor driver: executes motion plans using stepper motors
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
|
||||
and Philipp Tiefenbacher. */
|
||||
|
||||
#include "stepper.h"
|
||||
#include "Configuration.h"
|
||||
#include "Marlin.h"
|
||||
#include "planner.h"
|
||||
#include "pins.h"
|
||||
#include "fastio.h"
|
||||
#include "temperature.h"
|
||||
#include "ultralcd.h"
|
||||
|
||||
#include "speed_lookuptable.h"
|
||||
|
||||
// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer.
|
||||
// for debugging purposes only, should be disabled by default
|
||||
#ifdef DEBUG_STEPS
|
||||
volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
|
||||
volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1};
|
||||
#endif
|
||||
|
||||
|
||||
// intRes = intIn1 * intIn2 >> 16
|
||||
// uses:
|
||||
// r26 to store 0
|
||||
// r27 to store the byte 1 of the 24 bit result
|
||||
#define MultiU16X8toH16(intRes, charIn1, intIn2) \
|
||||
asm volatile ( \
|
||||
"clr r26 \n\t" \
|
||||
"mul %A1, %B2 \n\t" \
|
||||
"movw %A0, r0 \n\t" \
|
||||
"mul %A1, %A2 \n\t" \
|
||||
"add %A0, r1 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"lsr r0 \n\t" \
|
||||
"adc %A0, r26 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"clr r1 \n\t" \
|
||||
: \
|
||||
"=&r" (intRes) \
|
||||
: \
|
||||
"d" (charIn1), \
|
||||
"d" (intIn2) \
|
||||
: \
|
||||
"r26" \
|
||||
)
|
||||
|
||||
// intRes = longIn1 * longIn2 >> 24
|
||||
// uses:
|
||||
// r26 to store 0
|
||||
// r27 to store the byte 1 of the 48bit result
|
||||
#define MultiU24X24toH16(intRes, longIn1, longIn2) \
|
||||
asm volatile ( \
|
||||
"clr r26 \n\t" \
|
||||
"mul %A1, %B2 \n\t" \
|
||||
"mov r27, r1 \n\t" \
|
||||
"mul %B1, %C2 \n\t" \
|
||||
"movw %A0, r0 \n\t" \
|
||||
"mul %C1, %C2 \n\t" \
|
||||
"add %B0, r0 \n\t" \
|
||||
"mul %C1, %B2 \n\t" \
|
||||
"add %A0, r0 \n\t" \
|
||||
"adc %B0, r1 \n\t" \
|
||||
"mul %A1, %C2 \n\t" \
|
||||
"add r27, r0 \n\t" \
|
||||
"adc %A0, r1 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"mul %B1, %B2 \n\t" \
|
||||
"add r27, r0 \n\t" \
|
||||
"adc %A0, r1 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"mul %C1, %A2 \n\t" \
|
||||
"add r27, r0 \n\t" \
|
||||
"adc %A0, r1 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"mul %B1, %A2 \n\t" \
|
||||
"add r27, r1 \n\t" \
|
||||
"adc %A0, r26 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"lsr r27 \n\t" \
|
||||
"adc %A0, r26 \n\t" \
|
||||
"adc %B0, r26 \n\t" \
|
||||
"clr r1 \n\t" \
|
||||
: \
|
||||
"=&r" (intRes) \
|
||||
: \
|
||||
"d" (longIn1), \
|
||||
"d" (longIn2) \
|
||||
: \
|
||||
"r26" , "r27" \
|
||||
)
|
||||
|
||||
// Some useful constants
|
||||
|
||||
#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
|
||||
#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
|
||||
|
||||
static block_t *current_block; // A pointer to the block currently being traced
|
||||
|
||||
// Variables used by The Stepper Driver Interrupt
|
||||
static unsigned char out_bits; // The next stepping-bits to be output
|
||||
static long counter_x, // Counter variables for the bresenham line tracer
|
||||
counter_y,
|
||||
counter_z,
|
||||
counter_e;
|
||||
static unsigned long step_events_completed; // The number of step events executed in the current block
|
||||
#ifdef ADVANCE
|
||||
static long advance_rate, advance, final_advance = 0;
|
||||
static short old_advance = 0;
|
||||
static short e_steps;
|
||||
#endif
|
||||
static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
|
||||
static long acceleration_time, deceleration_time;
|
||||
//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
|
||||
static unsigned short acc_step_rate; // needed for deccelaration start point
|
||||
static char step_loops;
|
||||
|
||||
|
||||
// __________________________
|
||||
// /| |\ _________________ ^
|
||||
// / | | \ /| |\ |
|
||||
// / | | \ / | | \ s
|
||||
// / | | | | | \ p
|
||||
// / | | | | | \ e
|
||||
// +-----+------------------------+---+--+---------------+----+ e
|
||||
// | BLOCK 1 | BLOCK 2 | d
|
||||
//
|
||||
// time ----->
|
||||
//
|
||||
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
|
||||
// first block->accelerate_until step_events_completed, then keeps going at constant speed until
|
||||
// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
|
||||
// The slope of acceleration is calculated with the leib ramp alghorithm.
|
||||
|
||||
void st_wake_up() {
|
||||
// TCNT1 = 0;
|
||||
ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
}
|
||||
|
||||
inline unsigned short calc_timer(unsigned short step_rate) {
|
||||
unsigned short timer;
|
||||
if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
|
||||
|
||||
if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
|
||||
step_rate = step_rate >> 2;
|
||||
step_loops = 4;
|
||||
}
|
||||
else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
|
||||
step_rate = step_rate >> 1;
|
||||
step_loops = 2;
|
||||
}
|
||||
else {
|
||||
step_loops = 1;
|
||||
}
|
||||
|
||||
if(step_rate < 32) step_rate = 32;
|
||||
step_rate -= 32; // Correct for minimal speed
|
||||
if(step_rate >= (8*256)){ // higher step rate
|
||||
unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
|
||||
unsigned char tmp_step_rate = (step_rate & 0x00ff);
|
||||
unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
|
||||
MultiU16X8toH16(timer, tmp_step_rate, gain);
|
||||
timer = (unsigned short)pgm_read_word_near(table_address) - timer;
|
||||
}
|
||||
else { // lower step rates
|
||||
unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
|
||||
table_address += ((step_rate)>>1) & 0xfffc;
|
||||
timer = (unsigned short)pgm_read_word_near(table_address);
|
||||
timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
|
||||
}
|
||||
if(timer < 100) timer = 100;
|
||||
return timer;
|
||||
}
|
||||
|
||||
// Initializes the trapezoid generator from the current block. Called whenever a new
|
||||
// block begins.
|
||||
inline void trapezoid_generator_reset() {
|
||||
#ifdef ADVANCE
|
||||
advance = current_block->initial_advance;
|
||||
final_advance = current_block->final_advance;
|
||||
#endif
|
||||
deceleration_time = 0;
|
||||
// advance_rate = current_block->advance_rate;
|
||||
// step_rate to timer interval
|
||||
acc_step_rate = current_block->initial_rate;
|
||||
acceleration_time = calc_timer(acc_step_rate);
|
||||
OCR1A = acceleration_time;
|
||||
}
|
||||
|
||||
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
|
||||
// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
|
||||
ISR(TIMER1_COMPA_vect)
|
||||
{
|
||||
if(busy){ Serial.print(*(unsigned short *)OCR1A); Serial.println(" BUSY");
|
||||
return;
|
||||
} // The busy-flag is used to avoid reentering this interrupt
|
||||
|
||||
busy = true;
|
||||
sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
|
||||
|
||||
// If there is no current block, attempt to pop one from the buffer
|
||||
if (current_block == NULL) {
|
||||
// Anything in the buffer?
|
||||
current_block = plan_get_current_block();
|
||||
if (current_block != NULL) {
|
||||
trapezoid_generator_reset();
|
||||
counter_x = -(current_block->step_event_count >> 1);
|
||||
counter_y = counter_x;
|
||||
counter_z = counter_x;
|
||||
counter_e = counter_x;
|
||||
step_events_completed = 0;
|
||||
#ifdef ADVANCE
|
||||
e_steps = 0;
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
// DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
}
|
||||
}
|
||||
|
||||
if (current_block != NULL) {
|
||||
// Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
|
||||
out_bits = current_block->direction_bits;
|
||||
|
||||
#ifdef ADVANCE
|
||||
// Calculate E early.
|
||||
counter_e += current_block->steps_e;
|
||||
if (counter_e > 0) {
|
||||
counter_e -= current_block->step_event_count;
|
||||
if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
|
||||
CRITICAL_SECTION_START;
|
||||
e_steps--;
|
||||
CRITICAL_SECTION_END;
|
||||
}
|
||||
else {
|
||||
CRITICAL_SECTION_START;
|
||||
e_steps++;
|
||||
CRITICAL_SECTION_END;
|
||||
}
|
||||
}
|
||||
// Do E steps + advance steps
|
||||
CRITICAL_SECTION_START;
|
||||
e_steps += ((advance >> 16) - old_advance);
|
||||
CRITICAL_SECTION_END;
|
||||
old_advance = advance >> 16;
|
||||
#endif //ADVANCE
|
||||
|
||||
// Set direction en check limit switches
|
||||
if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
|
||||
WRITE(X_DIR_PIN, INVERT_X_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[X_AXIS]=-1;
|
||||
#endif
|
||||
#if X_MIN_PIN > -1
|
||||
if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) {
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else { // +direction
|
||||
WRITE(X_DIR_PIN,!INVERT_X_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[X_AXIS]=1;
|
||||
#endif
|
||||
#if X_MAX_PIN > -1
|
||||
if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x >0)){
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
|
||||
WRITE(Y_DIR_PIN,INVERT_Y_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[Y_AXIS]=-1;
|
||||
#endif
|
||||
#if Y_MIN_PIN > -1
|
||||
if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) {
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else { // +direction
|
||||
WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[Y_AXIS]=1;
|
||||
#endif
|
||||
#if Y_MAX_PIN > -1
|
||||
if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
|
||||
WRITE(Z_DIR_PIN,INVERT_Z_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[Z_AXIS]=-1;
|
||||
#endif
|
||||
#if Z_MIN_PIN > -1
|
||||
if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) {
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else { // +direction
|
||||
WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_direction[Z_AXIS]=1;
|
||||
#endif
|
||||
#if Z_MAX_PIN > -1
|
||||
if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z >0)){
|
||||
step_events_completed = current_block->step_event_count;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifndef ADVANCE
|
||||
if ((out_bits & (1<<E_AXIS)) != 0) // -direction
|
||||
WRITE(E_DIR_PIN,INVERT_E_DIR);
|
||||
else // +direction
|
||||
WRITE(E_DIR_PIN,!INVERT_E_DIR);
|
||||
#endif //!ADVANCE
|
||||
|
||||
for(char i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
|
||||
counter_x += current_block->steps_x;
|
||||
if (counter_x > 0) {
|
||||
WRITE(X_STEP_PIN, HIGH);
|
||||
counter_x -= current_block->step_event_count;
|
||||
WRITE(X_STEP_PIN, LOW);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_position[X_AXIS]+=count_direction[X_AXIS];
|
||||
#endif
|
||||
}
|
||||
|
||||
counter_y += current_block->steps_y;
|
||||
if (counter_y > 0) {
|
||||
WRITE(Y_STEP_PIN, HIGH);
|
||||
counter_y -= current_block->step_event_count;
|
||||
WRITE(Y_STEP_PIN, LOW);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_position[Y_AXIS]+=count_direction[Y_AXIS];
|
||||
#endif
|
||||
}
|
||||
|
||||
counter_z += current_block->steps_z;
|
||||
if (counter_z > 0) {
|
||||
WRITE(Z_STEP_PIN, HIGH);
|
||||
counter_z -= current_block->step_event_count;
|
||||
WRITE(Z_STEP_PIN, LOW);
|
||||
#ifdef DEBUG_STEPS
|
||||
count_position[Z_AXIS]+=count_direction[Z_AXIS];
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifndef ADVANCE
|
||||
counter_e += current_block->steps_e;
|
||||
if (counter_e > 0) {
|
||||
WRITE(E_STEP_PIN, HIGH);
|
||||
counter_e -= current_block->step_event_count;
|
||||
WRITE(E_STEP_PIN, LOW);
|
||||
}
|
||||
#endif //!ADVANCE
|
||||
step_events_completed += 1;
|
||||
if(step_events_completed >= current_block->step_event_count) break;
|
||||
}
|
||||
// Calculare new timer value
|
||||
unsigned short timer;
|
||||
unsigned short step_rate;
|
||||
if (step_events_completed <= current_block->accelerate_until) {
|
||||
MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
|
||||
acc_step_rate += current_block->initial_rate;
|
||||
|
||||
// upper limit
|
||||
if(acc_step_rate > current_block->nominal_rate)
|
||||
acc_step_rate = current_block->nominal_rate;
|
||||
|
||||
// step_rate to timer interval
|
||||
timer = calc_timer(acc_step_rate);
|
||||
#ifdef ADVANCE
|
||||
advance += advance_rate;
|
||||
#endif
|
||||
acceleration_time += timer;
|
||||
OCR1A = timer;
|
||||
}
|
||||
else if (step_events_completed > current_block->decelerate_after) {
|
||||
MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
|
||||
|
||||
if(step_rate > acc_step_rate) { // Check step_rate stays positive
|
||||
step_rate = current_block->final_rate;
|
||||
}
|
||||
else {
|
||||
step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
|
||||
}
|
||||
|
||||
// lower limit
|
||||
if(step_rate < current_block->final_rate)
|
||||
step_rate = current_block->final_rate;
|
||||
|
||||
// step_rate to timer interval
|
||||
timer = calc_timer(step_rate);
|
||||
#ifdef ADVANCE
|
||||
advance -= advance_rate;
|
||||
if(advance < final_advance)
|
||||
advance = final_advance;
|
||||
#endif //ADVANCE
|
||||
deceleration_time += timer;
|
||||
OCR1A = timer;
|
||||
}
|
||||
// If current block is finished, reset pointer
|
||||
if (step_events_completed >= current_block->step_event_count) {
|
||||
current_block = NULL;
|
||||
plan_discard_current_block();
|
||||
}
|
||||
}
|
||||
cli(); // disable interrupts
|
||||
busy=false;
|
||||
}
|
||||
|
||||
#ifdef ADVANCE
|
||||
|
||||
unsigned char old_OCR0A;
|
||||
// Timer interrupt for E. e_steps is set in the main routine;
|
||||
// Timer 0 is shared with millies
|
||||
ISR(TIMER0_COMPA_vect)
|
||||
{
|
||||
// Critical section needed because Timer 1 interrupt has higher priority.
|
||||
// The pin set functions are placed on trategic position to comply with the stepper driver timing.
|
||||
WRITE(E_STEP_PIN, LOW);
|
||||
// Set E direction (Depends on E direction + advance)
|
||||
if (e_steps < 0) {
|
||||
WRITE(E_DIR_PIN,INVERT_E_DIR);
|
||||
e_steps++;
|
||||
WRITE(E_STEP_PIN, HIGH);
|
||||
}
|
||||
if (e_steps > 0) {
|
||||
WRITE(E_DIR_PIN,!INVERT_E_DIR);
|
||||
e_steps--;
|
||||
WRITE(E_STEP_PIN, HIGH);
|
||||
}
|
||||
old_OCR0A += 25; // 10kHz interrupt
|
||||
OCR0A = old_OCR0A;
|
||||
}
|
||||
#endif // ADVANCE
|
||||
|
||||
void st_init()
|
||||
{
|
||||
//Initialize Dir Pins
|
||||
#if X_DIR_PIN > -1
|
||||
SET_OUTPUT(X_DIR_PIN);
|
||||
#endif
|
||||
#if Y_DIR_PIN > -1
|
||||
SET_OUTPUT(Y_DIR_PIN);
|
||||
#endif
|
||||
#if Z_DIR_PIN > -1
|
||||
SET_OUTPUT(Z_DIR_PIN);
|
||||
#endif
|
||||
#if E_DIR_PIN > -1
|
||||
SET_OUTPUT(E_DIR_PIN);
|
||||
#endif
|
||||
|
||||
//Initialize Enable Pins - steppers default to disabled.
|
||||
|
||||
#if (X_ENABLE_PIN > -1)
|
||||
SET_OUTPUT(X_ENABLE_PIN);
|
||||
if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
|
||||
#endif
|
||||
#if (Y_ENABLE_PIN > -1)
|
||||
SET_OUTPUT(Y_ENABLE_PIN);
|
||||
if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
|
||||
#endif
|
||||
#if (Z_ENABLE_PIN > -1)
|
||||
SET_OUTPUT(Z_ENABLE_PIN);
|
||||
if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
|
||||
#endif
|
||||
#if (E_ENABLE_PIN > -1)
|
||||
SET_OUTPUT(E_ENABLE_PIN);
|
||||
if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH);
|
||||
#endif
|
||||
|
||||
//endstops and pullups
|
||||
#ifdef ENDSTOPPULLUPS
|
||||
#if X_MIN_PIN > -1
|
||||
SET_INPUT(X_MIN_PIN);
|
||||
WRITE(X_MIN_PIN,HIGH);
|
||||
#endif
|
||||
#if X_MAX_PIN > -1
|
||||
SET_INPUT(X_MAX_PIN);
|
||||
WRITE(X_MAX_PIN,HIGH);
|
||||
#endif
|
||||
#if Y_MIN_PIN > -1
|
||||
SET_INPUT(Y_MIN_PIN);
|
||||
WRITE(Y_MIN_PIN,HIGH);
|
||||
#endif
|
||||
#if Y_MAX_PIN > -1
|
||||
SET_INPUT(Y_MAX_PIN);
|
||||
WRITE(Y_MAX_PIN,HIGH);
|
||||
#endif
|
||||
#if Z_MIN_PIN > -1
|
||||
SET_INPUT(Z_MIN_PIN);
|
||||
WRITE(Z_MIN_PIN,HIGH);
|
||||
#endif
|
||||
#if Z_MAX_PIN > -1
|
||||
SET_INPUT(Z_MAX_PIN);
|
||||
WRITE(Z_MAX_PIN,HIGH);
|
||||
#endif
|
||||
#else //ENDSTOPPULLUPS
|
||||
#if X_MIN_PIN > -1
|
||||
SET_INPUT(X_MIN_PIN);
|
||||
#endif
|
||||
#if X_MAX_PIN > -1
|
||||
SET_INPUT(X_MAX_PIN);
|
||||
#endif
|
||||
#if Y_MIN_PIN > -1
|
||||
SET_INPUT(Y_MIN_PIN);
|
||||
#endif
|
||||
#if Y_MAX_PIN > -1
|
||||
SET_INPUT(Y_MAX_PIN);
|
||||
#endif
|
||||
#if Z_MIN_PIN > -1
|
||||
SET_INPUT(Z_MIN_PIN);
|
||||
#endif
|
||||
#if Z_MAX_PIN > -1
|
||||
SET_INPUT(Z_MAX_PIN);
|
||||
#endif
|
||||
#endif //ENDSTOPPULLUPS
|
||||
|
||||
|
||||
//Initialize Step Pins
|
||||
#if (X_STEP_PIN > -1)
|
||||
SET_OUTPUT(X_STEP_PIN);
|
||||
#endif
|
||||
#if (Y_STEP_PIN > -1)
|
||||
SET_OUTPUT(Y_STEP_PIN);
|
||||
#endif
|
||||
#if (Z_STEP_PIN > -1)
|
||||
SET_OUTPUT(Z_STEP_PIN);
|
||||
#endif
|
||||
#if (E_STEP_PIN > -1)
|
||||
SET_OUTPUT(E_STEP_PIN);
|
||||
#endif
|
||||
|
||||
// waveform generation = 0100 = CTC
|
||||
TCCR1B &= ~(1<<WGM13);
|
||||
TCCR1B |= (1<<WGM12);
|
||||
TCCR1A &= ~(1<<WGM11);
|
||||
TCCR1A &= ~(1<<WGM10);
|
||||
|
||||
// output mode = 00 (disconnected)
|
||||
TCCR1A &= ~(3<<COM1A0);
|
||||
TCCR1A &= ~(3<<COM1B0);
|
||||
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer
|
||||
|
||||
OCR1A = 0x4000;
|
||||
DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
|
||||
#ifdef ADVANCE
|
||||
e_steps = 0;
|
||||
TIMSK0 |= (1<<OCIE0A);
|
||||
#endif //ADVANCE
|
||||
sei();
|
||||
}
|
||||
|
||||
// Block until all buffered steps are executed
|
||||
void st_synchronize()
|
||||
{
|
||||
while(plan_get_current_block()) {
|
||||
manage_heater();
|
||||
manage_inactivity(1);
|
||||
LCD_STATUS;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
/*
|
||||
stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef stepper_h
|
||||
#define stepper_h
|
||||
// Initialize and start the stepper motor subsystem
|
||||
void st_init();
|
||||
|
||||
// Block until all buffered steps are executed
|
||||
void st_synchronize();
|
||||
|
||||
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
|
||||
// to notify the subsystem that it is time to go to work.
|
||||
void st_wake_up();
|
||||
|
||||
// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer.
|
||||
// for debugging purposes only, should be disabled by default
|
||||
#ifdef DEBUG_STEPS
|
||||
extern volatile long count_position[NUM_AXIS];
|
||||
extern volatile int count_direction[NUM_AXIS];
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,84 @@
|
||||
/*
|
||||
Streaming.h - Arduino library for supporting the << streaming operator
|
||||
Copyright (c) 2010 Mikal Hart. All rights reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#ifndef ARDUINO_STREAMING
|
||||
#define ARDUINO_STREAMING
|
||||
|
||||
//#include <WProgram.h>
|
||||
|
||||
#define STREAMING_LIBRARY_VERSION 4
|
||||
|
||||
// Generic template
|
||||
template<class T>
|
||||
inline Print &operator <<(Print &stream, T arg)
|
||||
{ stream.print(arg); return stream; }
|
||||
|
||||
struct _BASED
|
||||
{
|
||||
long val;
|
||||
int base;
|
||||
_BASED(long v, int b): val(v), base(b)
|
||||
{}
|
||||
};
|
||||
|
||||
#define _HEX(a) _BASED(a, HEX)
|
||||
#define _DEC(a) _BASED(a, DEC)
|
||||
#define _OCT(a) _BASED(a, OCT)
|
||||
#define _BIN(a) _BASED(a, BIN)
|
||||
#define _BYTE(a) _BASED(a, BYTE)
|
||||
|
||||
// Specialization for class _BASED
|
||||
// Thanks to Arduino forum user Ben Combee who suggested this
|
||||
// clever technique to allow for expressions like
|
||||
// Serial << _HEX(a);
|
||||
|
||||
inline Print &operator <<(Print &obj, const _BASED &arg)
|
||||
{ obj.print(arg.val, arg.base); return obj; }
|
||||
|
||||
#if ARDUINO >= 18
|
||||
// Specialization for class _FLOAT
|
||||
// Thanks to Michael Margolis for suggesting a way
|
||||
// to accommodate Arduino 0018's floating point precision
|
||||
// feature like this:
|
||||
// Serial << _FLOAT(gps_latitude, 6); // 6 digits of precision
|
||||
|
||||
struct _FLOAT
|
||||
{
|
||||
float val;
|
||||
int digits;
|
||||
_FLOAT(double v, int d): val(v), digits(d)
|
||||
{}
|
||||
};
|
||||
|
||||
inline Print &operator <<(Print &obj, const _FLOAT &arg)
|
||||
{ obj.print(arg.val, arg.digits); return obj; }
|
||||
#endif
|
||||
|
||||
// Specialization for enum _EndLineCode
|
||||
// Thanks to Arduino forum user Paul V. who suggested this
|
||||
// clever technique to allow for expressions like
|
||||
// Serial << "Hello!" << endl;
|
||||
|
||||
enum _EndLineCode { endl };
|
||||
|
||||
inline Print &operator <<(Print &obj, _EndLineCode arg)
|
||||
{ obj.println(); return obj; }
|
||||
|
||||
#endif
|
||||
|
||||
@ -0,0 +1,476 @@
|
||||
/*
|
||||
temperature.c - temperature control
|
||||
Part of Marlin
|
||||
|
||||
Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
This firmware is a mashup between Sprinter and grbl.
|
||||
(https://github.com/kliment/Sprinter)
|
||||
(https://github.com/simen/grbl/tree)
|
||||
|
||||
It has preliminary support for Matthew Roberts advance algorithm
|
||||
http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
|
||||
|
||||
This firmware is optimized for gen6 electronics.
|
||||
*/
|
||||
|
||||
#include "fastio.h"
|
||||
#include "Configuration.h"
|
||||
#include "pins.h"
|
||||
#include "Marlin.h"
|
||||
#include "ultralcd.h"
|
||||
#include "streaming.h"
|
||||
#include "temperature.h"
|
||||
|
||||
int target_bed_raw = 0;
|
||||
int current_bed_raw = 0;
|
||||
|
||||
int target_raw[3] = {0, 0, 0};
|
||||
int current_raw[3] = {0, 0, 0};
|
||||
unsigned char temp_meas_ready = false;
|
||||
|
||||
unsigned long previous_millis_heater, previous_millis_bed_heater;
|
||||
|
||||
#ifdef PIDTEMP
|
||||
double temp_iState = 0;
|
||||
double temp_dState = 0;
|
||||
double pTerm;
|
||||
double iTerm;
|
||||
double dTerm;
|
||||
//int output;
|
||||
double pid_error;
|
||||
double temp_iState_min;
|
||||
double temp_iState_max;
|
||||
double pid_setpoint = 0.0;
|
||||
double pid_input;
|
||||
double pid_output;
|
||||
bool pid_reset;
|
||||
float HeaterPower;
|
||||
|
||||
float Kp=DEFAULT_Kp;
|
||||
float Ki=DEFAULT_Ki;
|
||||
float Kd=DEFAULT_Kd;
|
||||
float Kc=DEFAULT_Kc;
|
||||
#endif //PIDTEMP
|
||||
|
||||
#ifdef MINTEMP
|
||||
int minttemp = temp2analog(MINTEMP);
|
||||
#endif //MINTEMP
|
||||
#ifdef MAXTEMP
|
||||
int maxttemp = temp2analog(MAXTEMP);
|
||||
#endif //MAXTEMP
|
||||
|
||||
#ifdef BED_MINTEMP
|
||||
int bed_minttemp = temp2analog(BED_MINTEMP);
|
||||
#endif //BED_MINTEMP
|
||||
#ifdef BED_MAXTEMP
|
||||
int bed_maxttemp = temp2analog(BED_MAXTEMP);
|
||||
#endif //BED_MAXTEMP
|
||||
|
||||
void manage_heater()
|
||||
{
|
||||
#ifdef USE_WATCHDOG
|
||||
wd_reset();
|
||||
#endif
|
||||
|
||||
float pid_input;
|
||||
float pid_output;
|
||||
if(temp_meas_ready == true) {
|
||||
|
||||
CRITICAL_SECTION_START;
|
||||
temp_meas_ready = false;
|
||||
CRITICAL_SECTION_END;
|
||||
|
||||
#ifdef PIDTEMP
|
||||
pid_input = analog2temp(current_raw[0]);
|
||||
|
||||
#ifndef PID_OPENLOOP
|
||||
pid_error = pid_setpoint - pid_input;
|
||||
if(pid_error > 10){
|
||||
pid_output = PID_MAX;
|
||||
pid_reset = true;
|
||||
}
|
||||
else if(pid_error < -10) {
|
||||
pid_output = 0;
|
||||
pid_reset = true;
|
||||
}
|
||||
else {
|
||||
if(pid_reset == true) {
|
||||
temp_iState = 0.0;
|
||||
pid_reset = false;
|
||||
}
|
||||
pTerm = Kp * pid_error;
|
||||
temp_iState += pid_error;
|
||||
temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
|
||||
iTerm = Ki * temp_iState;
|
||||
#define K1 0.95
|
||||
#define K2 (1.0-K1)
|
||||
dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm);
|
||||
temp_dState = pid_input;
|
||||
pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX);
|
||||
}
|
||||
#endif //PID_OPENLOOP
|
||||
#ifdef PID_DEBUG
|
||||
Serial.print(" Input ");
|
||||
Serial.print(pid_input);
|
||||
Serial.print(" Output ");
|
||||
Serial.print(pid_output);
|
||||
Serial.print(" pTerm ");
|
||||
Serial.print(pTerm);
|
||||
Serial.print(" iTerm ");
|
||||
Serial.print(iTerm);
|
||||
Serial.print(" dTerm ");
|
||||
Serial.print(dTerm);
|
||||
Serial.println();
|
||||
#endif //PID_DEBUG
|
||||
analogWrite(HEATER_0_PIN, pid_output);
|
||||
#endif //PIDTEMP
|
||||
|
||||
#ifndef PIDTEMP
|
||||
if(current_raw[0] >= target_raw[0])
|
||||
{
|
||||
WRITE(HEATER_0_PIN,LOW);
|
||||
}
|
||||
else
|
||||
{
|
||||
WRITE(HEATER_0_PIN,HIGH);
|
||||
}
|
||||
#endif
|
||||
|
||||
if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
|
||||
return;
|
||||
previous_millis_bed_heater = millis();
|
||||
|
||||
#if TEMP_1_PIN > -1
|
||||
if(current_raw[1] >= target_raw[1])
|
||||
{
|
||||
WRITE(HEATER_1_PIN,LOW);
|
||||
}
|
||||
else
|
||||
{
|
||||
WRITE(HEATER_1_PIN,HIGH);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// Takes hot end temperature value as input and returns corresponding raw value.
|
||||
// For a thermistor, it uses the RepRap thermistor temp table.
|
||||
// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
|
||||
// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
|
||||
float temp2analog(int celsius) {
|
||||
#ifdef HEATER_USES_THERMISTOR
|
||||
int raw = 0;
|
||||
byte i;
|
||||
|
||||
for (i=1; i<NUMTEMPS; i++)
|
||||
{
|
||||
if (temptable[i][1] < celsius)
|
||||
{
|
||||
raw = temptable[i-1][0] +
|
||||
(celsius - temptable[i-1][1]) *
|
||||
(temptable[i][0] - temptable[i-1][0]) /
|
||||
(temptable[i][1] - temptable[i-1][1]);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Overflow: Set to last value in the table
|
||||
if (i == NUMTEMPS) raw = temptable[i-1][0];
|
||||
|
||||
return (1023 * OVERSAMPLENR) - raw;
|
||||
#elif defined HEATER_USES_AD595
|
||||
return celsius * (1024.0 / (5.0 * 100.0) ) * OVERSAMPLENR;
|
||||
#endif
|
||||
}
|
||||
|
||||
// Takes bed temperature value as input and returns corresponding raw value.
|
||||
// For a thermistor, it uses the RepRap thermistor temp table.
|
||||
// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value.
|
||||
// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware.
|
||||
float temp2analogBed(int celsius) {
|
||||
#ifdef BED_USES_THERMISTOR
|
||||
|
||||
int raw = 0;
|
||||
byte i;
|
||||
|
||||
for (i=1; i<BNUMTEMPS; i++)
|
||||
{
|
||||
if (bedtemptable[i][1] < celsius)
|
||||
{
|
||||
raw = bedtemptable[i-1][0] +
|
||||
(celsius - bedtemptable[i-1][1]) *
|
||||
(bedtemptable[i][0] - bedtemptable[i-1][0]) /
|
||||
(bedtemptable[i][1] - bedtemptable[i-1][1]);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Overflow: Set to last value in the table
|
||||
if (i == BNUMTEMPS) raw = bedtemptable[i-1][0];
|
||||
|
||||
return (1023 * OVERSAMPLENR) - raw;
|
||||
#elif defined BED_USES_AD595
|
||||
return celsius * (1024.0 / (5.0 * 100.0) ) * OVERSAMPLENR;
|
||||
#endif
|
||||
}
|
||||
|
||||
// Derived from RepRap FiveD extruder::getTemperature()
|
||||
// For hot end temperature measurement.
|
||||
float analog2temp(int raw) {
|
||||
#ifdef HEATER_USES_THERMISTOR
|
||||
int celsius = 0;
|
||||
byte i;
|
||||
raw = (1023 * OVERSAMPLENR) - raw;
|
||||
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];
|
||||
|
||||
return celsius;
|
||||
#elif defined HEATER_USES_AD595
|
||||
return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;
|
||||
#endif
|
||||
}
|
||||
|
||||
// Derived from RepRap FiveD extruder::getTemperature()
|
||||
// For bed temperature measurement.
|
||||
float analog2tempBed(int raw) {
|
||||
#ifdef BED_USES_THERMISTOR
|
||||
int celsius = 0;
|
||||
byte i;
|
||||
|
||||
raw = (1023 * OVERSAMPLENR) - raw;
|
||||
|
||||
for (i=1; i<NUMTEMPS; i++)
|
||||
{
|
||||
if (bedtemptable[i][0] > raw)
|
||||
{
|
||||
celsius = bedtemptable[i-1][1] +
|
||||
(raw - bedtemptable[i-1][0]) *
|
||||
(bedtemptable[i][1] - bedtemptable[i-1][1]) /
|
||||
(bedtemptable[i][0] - bedtemptable[i-1][0]);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Overflow: Set to last value in the table
|
||||
if (i == NUMTEMPS) celsius = bedtemptable[i-1][1];
|
||||
|
||||
return celsius;
|
||||
|
||||
#elif defined BED_USES_AD595
|
||||
return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR;
|
||||
#endif
|
||||
}
|
||||
|
||||
void tp_init()
|
||||
{
|
||||
#if (HEATER_0_PIN > -1)
|
||||
SET_OUTPUT(HEATER_0_PIN);
|
||||
#endif
|
||||
#if (HEATER_1_PIN > -1)
|
||||
SET_OUTPUT(HEATER_1_PIN);
|
||||
#endif
|
||||
#if (HEATER_2_PIN > -1)
|
||||
SET_OUTPUT(HEATER_2_PIN);
|
||||
#endif
|
||||
|
||||
#ifdef PIDTEMP
|
||||
temp_iState_min = 0.0;
|
||||
temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki;
|
||||
#endif //PIDTEMP
|
||||
|
||||
// Set analog inputs
|
||||
ADCSRA = 1<<ADEN | 1<<ADSC | 1<<ADIF | 0x07;
|
||||
|
||||
// Use timer0 for temperature measurement
|
||||
// Interleave temperature interrupt with millies interrupt
|
||||
OCR0B = 128;
|
||||
TIMSK0 |= (1<<OCIE0B);
|
||||
}
|
||||
|
||||
static unsigned char temp_count = 0;
|
||||
static unsigned long raw_temp_0_value = 0;
|
||||
static unsigned long raw_temp_1_value = 0;
|
||||
static unsigned long raw_temp_2_value = 0;
|
||||
static unsigned char temp_state = 0;
|
||||
|
||||
// Timer 0 is shared with millies
|
||||
ISR(TIMER0_COMPB_vect)
|
||||
{
|
||||
switch(temp_state) {
|
||||
case 0: // Prepare TEMP_0
|
||||
#if (TEMP_0_PIN > -1)
|
||||
#if TEMP_0_PIN < 8
|
||||
DIDR0 = 1 << TEMP_0_PIN;
|
||||
#else
|
||||
DIDR2 = 1<<(TEMP_0_PIN - 8);
|
||||
ADCSRB = 1<<MUX5;
|
||||
#endif
|
||||
ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));
|
||||
ADCSRA |= 1<<ADSC; // Start conversion
|
||||
#endif
|
||||
#ifdef ULTIPANEL
|
||||
buttons_check();
|
||||
#endif
|
||||
temp_state = 1;
|
||||
break;
|
||||
case 1: // Measure TEMP_0
|
||||
#if (TEMP_0_PIN > -1)
|
||||
raw_temp_0_value += ADC;
|
||||
#endif
|
||||
temp_state = 2;
|
||||
break;
|
||||
case 2: // Prepare TEMP_1
|
||||
#if (TEMP_1_PIN > -1)
|
||||
#if TEMP_1_PIN < 7
|
||||
DIDR0 = 1<<TEMP_1_PIN;
|
||||
#else
|
||||
DIDR2 = 1<<(TEMP_1_PIN - 8);
|
||||
ADCSRB = 1<<MUX5;
|
||||
#endif
|
||||
ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
|
||||
ADCSRA |= 1<<ADSC; // Start conversion
|
||||
#endif
|
||||
#ifdef ULTIPANEL
|
||||
buttons_check();
|
||||
#endif
|
||||
temp_state = 3;
|
||||
break;
|
||||
case 3: // Measure TEMP_1
|
||||
#if (TEMP_1_PIN > -1)
|
||||
raw_temp_1_value += ADC;
|
||||
#endif
|
||||
temp_state = 4;
|
||||
break;
|
||||
case 4: // Prepare TEMP_2
|
||||
#if (TEMP_2_PIN > -1)
|
||||
#if TEMP_2_PIN < 7
|
||||
DIDR0 = 1 << TEMP_2_PIN;
|
||||
#else
|
||||
DIDR2 = 1<<(TEMP_2_PIN - 8);
|
||||
ADCSRB = 1<<MUX5;
|
||||
#endif
|
||||
ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
|
||||
ADCSRA |= 1<<ADSC; // Start conversion
|
||||
#endif
|
||||
#ifdef ULTIPANEL
|
||||
buttons_check();
|
||||
#endif
|
||||
temp_state = 5;
|
||||
break;
|
||||
case 5: // Measure TEMP_2
|
||||
#if (TEMP_2_PIN > -1)
|
||||
raw_temp_2_value += ADC;
|
||||
#endif
|
||||
temp_state = 0;
|
||||
temp_count++;
|
||||
break;
|
||||
default:
|
||||
Serial.println("!! Temp measurement error !!");
|
||||
break;
|
||||
}
|
||||
|
||||
if(temp_count >= 16) // 6 ms * 16 = 96ms.
|
||||
{
|
||||
#ifdef HEATER_USES_AD595
|
||||
current_raw[0] = raw_temp_0_value;
|
||||
current_raw[2] = raw_temp_2_value;
|
||||
#else
|
||||
current_raw[0] = 16383 - raw_temp_0_value;
|
||||
current_raw[2] = 16383 - raw_temp_2_value;
|
||||
#endif
|
||||
|
||||
#ifdef BED_USES_AD595
|
||||
current_raw[1] = raw_temp_1_value;
|
||||
#else
|
||||
current_raw[1] = 16383 - raw_temp_1_value;
|
||||
#endif
|
||||
|
||||
temp_meas_ready = true;
|
||||
temp_count = 0;
|
||||
raw_temp_0_value = 0;
|
||||
raw_temp_1_value = 0;
|
||||
raw_temp_2_value = 0;
|
||||
#ifdef MAXTEMP
|
||||
#if (HEATER_0_PIN > -1)
|
||||
if(current_raw[0] >= maxttemp) {
|
||||
target_raw[0] = 0;
|
||||
analogWrite(HEATER_0_PIN, 0);
|
||||
Serial.println("!! Temperature extruder 0 switched off. MAXTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#if (HEATER_2_PIN > -1)
|
||||
if(current_raw[2] >= maxttemp) {
|
||||
target_raw[2] = 0;
|
||||
analogWrite(HEATER_2_PIN, 0);
|
||||
Serial.println("!! Temperature extruder 1 switched off. MAXTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#endif //MAXTEMP
|
||||
#ifdef MINTEMP
|
||||
#if (HEATER_0_PIN > -1)
|
||||
if(current_raw[0] <= minttemp) {
|
||||
target_raw[0] = 0;
|
||||
analogWrite(HEATER_0_PIN, 0);
|
||||
Serial.println("!! Temperature extruder 0 switched off. MINTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#if (HEATER_2_PIN > -1)
|
||||
if(current_raw[2] <= minttemp) {
|
||||
target_raw[2] = 0;
|
||||
analogWrite(HEATER_2_PIN, 0);
|
||||
Serial.println("!! Temperature extruder 1 switched off. MINTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#endif //MAXTEMP
|
||||
#ifdef BED_MINTEMP
|
||||
#if (HEATER_1_PIN > -1)
|
||||
if(current_raw[1] <= bed_minttemp) {
|
||||
target_raw[1] = 0;
|
||||
WRITE(HEATER_1_PIN, 0);
|
||||
Serial.println("!! Temperatur heated bed switched off. MINTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
#ifdef BED_MAXTEMP
|
||||
#if (HEATER_1_PIN > -1)
|
||||
if(current_raw[1] >= bed_maxttemp) {
|
||||
target_raw[1] = 0;
|
||||
WRITE(HEATER_1_PIN, 0);
|
||||
Serial.println("!! Temperature heated bed switched off. MAXTEMP triggered !!");
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,55 @@
|
||||
/*
|
||||
temperature.h - temperature controller
|
||||
Part of Marlin
|
||||
|
||||
Copyright (c) 2011 Erik van der Zalm
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef temperature_h
|
||||
#define temperature_h
|
||||
|
||||
void manage_inactivity(byte debug);
|
||||
|
||||
void tp_init();
|
||||
void manage_heater();
|
||||
//int temp2analogu(int celsius, const short table[][2], int numtemps);
|
||||
//float analog2tempu(int raw, const short table[][2], int numtemps);
|
||||
float temp2analog(int celsius);
|
||||
float temp2analogBed(int celsius);
|
||||
float analog2temp(int raw);
|
||||
float analog2tempBed(int raw);
|
||||
|
||||
#ifdef HEATER_USES_THERMISTOR
|
||||
#define HEATERSOURCE 1
|
||||
#endif
|
||||
#ifdef BED_USES_THERMISTOR
|
||||
#define BEDSOURCE 1
|
||||
#endif
|
||||
|
||||
//#define temp2analogh( c ) temp2analogu((c),temptable,NUMTEMPS)
|
||||
//#define analog2temp( c ) analog2tempu((c),temptable,NUMTEMPS
|
||||
|
||||
|
||||
extern float Kp;
|
||||
extern float Ki;
|
||||
extern float Kd;
|
||||
extern float Kc;
|
||||
|
||||
extern int target_raw[3];
|
||||
extern int current_raw[3];
|
||||
extern double pid_setpoint;
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,156 @@
|
||||
#ifndef __ULTRALCDH
|
||||
#define __ULTRALCDH
|
||||
#include "Configuration.h"
|
||||
|
||||
#ifdef ULTRA_LCD
|
||||
|
||||
void lcd_status();
|
||||
void lcd_init();
|
||||
void lcd_status(const char* message);
|
||||
void beep();
|
||||
void buttons_check();
|
||||
#define LCDSTATUSRIGHT
|
||||
|
||||
#define LCD_UPDATE_INTERVAL 100
|
||||
#define STATUSTIMEOUT 15000
|
||||
|
||||
#include "Configuration.h"
|
||||
|
||||
#include <LiquidCrystal.h>
|
||||
extern LiquidCrystal lcd;
|
||||
|
||||
//lcd display size
|
||||
|
||||
#ifdef NEWPANEL
|
||||
//arduino pin witch triggers an piezzo beeper
|
||||
#define BEEPER 18
|
||||
|
||||
#define LCD_PINS_RS 20
|
||||
#define LCD_PINS_ENABLE 17
|
||||
#define LCD_PINS_D4 16
|
||||
#define LCD_PINS_D5 21
|
||||
#define LCD_PINS_D6 5
|
||||
#define LCD_PINS_D7 6
|
||||
|
||||
//buttons are directly attached
|
||||
#define BTN_EN1 40
|
||||
#define BTN_EN2 42
|
||||
#define BTN_ENC 19 //the click
|
||||
|
||||
#define BLEN_C 2
|
||||
#define BLEN_B 1
|
||||
#define BLEN_A 0
|
||||
|
||||
#define SDCARDDETECT 38
|
||||
|
||||
#define EN_C (1<<BLEN_C)
|
||||
#define EN_B (1<<BLEN_B)
|
||||
#define EN_A (1<<BLEN_A)
|
||||
|
||||
//encoder rotation values
|
||||
#define encrot0 0
|
||||
#define encrot1 2
|
||||
#define encrot2 3
|
||||
#define encrot3 1
|
||||
|
||||
|
||||
#define CLICKED (buttons&EN_C)
|
||||
#define BLOCK {blocking=millis()+blocktime;}
|
||||
#define CARDINSERTED (READ(SDCARDDETECT)==0)
|
||||
|
||||
#else
|
||||
//arduino pin witch triggers an piezzo beeper
|
||||
#define BEEPER 18
|
||||
|
||||
//buttons are attached to a shift register
|
||||
#define SHIFT_CLK 38
|
||||
#define SHIFT_LD 42
|
||||
#define SHIFT_OUT 40
|
||||
#define SHIFT_EN 17
|
||||
|
||||
#define LCD_PINS_RS 16
|
||||
#define LCD_PINS_ENABLE 5
|
||||
#define LCD_PINS_D4 6
|
||||
#define LCD_PINS_D5 21
|
||||
#define LCD_PINS_D6 20
|
||||
#define LCD_PINS_D7 19
|
||||
|
||||
//bits in the shift register that carry the buttons for:
|
||||
// left up center down right red
|
||||
#define BL_LE 7
|
||||
#define BL_UP 6
|
||||
#define BL_MI 5
|
||||
#define BL_DW 4
|
||||
#define BL_RI 3
|
||||
#define BL_ST 2
|
||||
|
||||
#define BLEN_B 1
|
||||
#define BLEN_A 0
|
||||
|
||||
//encoder rotation values
|
||||
#define encrot0 0
|
||||
#define encrot1 2
|
||||
#define encrot2 3
|
||||
#define encrot3 1
|
||||
|
||||
//atomatic, do not change
|
||||
#define B_LE (1<<BL_LE)
|
||||
#define B_UP (1<<BL_UP)
|
||||
#define B_MI (1<<BL_MI)
|
||||
#define B_DW (1<<BL_DW)
|
||||
#define B_RI (1<<BL_RI)
|
||||
#define B_ST (1<<BL_ST)
|
||||
#define EN_B (1<<BLEN_B)
|
||||
#define EN_A (1<<BLEN_A)
|
||||
|
||||
#define CLICKED ((buttons&B_MI)||(buttons&B_ST))
|
||||
#define BLOCK {blocking[BL_MI]=millis()+blocktime;blocking[BL_ST]=millis()+blocktime;}
|
||||
|
||||
#endif
|
||||
// blocking time for recognizing a new keypress of one key, ms
|
||||
#define blocktime 500
|
||||
#define lcdslow 5
|
||||
enum MainStatus{Main_Status, Main_Menu, Main_Prepare, Main_Control, Main_SD};
|
||||
|
||||
class MainMenu{
|
||||
public:
|
||||
MainMenu();
|
||||
void update();
|
||||
void getfilename(const uint8_t nr);
|
||||
uint8_t activeline;
|
||||
MainStatus status;
|
||||
uint8_t displayStartingRow;
|
||||
|
||||
void showStatus();
|
||||
void showMainMenu();
|
||||
void showPrepare();
|
||||
void showControl();
|
||||
void showSD();
|
||||
bool force_lcd_update;
|
||||
int lastencoderpos;
|
||||
int8_t lineoffset;
|
||||
int8_t lastlineoffset;
|
||||
char filename[11];
|
||||
bool linechanging;
|
||||
};
|
||||
|
||||
char *fillto(int8_t n,char *c);
|
||||
char *ftostr51(const float &x);
|
||||
char *ftostr31(const float &x);
|
||||
char *ftostr3(const float &x);
|
||||
|
||||
|
||||
|
||||
#define LCD_MESSAGE(x) lcd_status(x);
|
||||
#define LCD_STATUS lcd_status()
|
||||
#else //no lcd
|
||||
#define LCD_STATUS
|
||||
#define LCD_MESSAGE(x)
|
||||
#endif
|
||||
|
||||
#ifndef ULTIPANEL
|
||||
#define CLICKED false
|
||||
#define BLOCK ;
|
||||
#endif
|
||||
#endif //ULTRALCD
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,176 +0,0 @@
|
||||
/*
|
||||
wiring.c - Partial implementation of the Wiring API for the ATmega8.
|
||||
Part of Arduino - http://www.arduino.cc/
|
||||
|
||||
Copyright (c) 2005-2006 David A. Mellis
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General
|
||||
Public License along with this library; if not, write to the
|
||||
Free Software Foundation, Inc., 59 Temple Place, Suite 330,
|
||||
Boston, MA 02111-1307 USA
|
||||
|
||||
$Id: wiring.c 388 2008-03-08 22:05:23Z mellis $
|
||||
*/
|
||||
|
||||
#include "wiring_private.h"
|
||||
|
||||
volatile unsigned long timer0_millis = 0;
|
||||
|
||||
SIGNAL(TIMER0_OVF_vect)
|
||||
{
|
||||
// timer 0 prescale factor is 64 and the timer overflows at 256
|
||||
timer0_millis++;
|
||||
}
|
||||
|
||||
unsigned long millis()
|
||||
{
|
||||
unsigned long m;
|
||||
uint8_t oldSREG = SREG;
|
||||
|
||||
// disable interrupts while we read timer0_millis or we might get an
|
||||
// inconsistent value (e.g. in the middle of the timer0_millis++)
|
||||
cli();
|
||||
m = timer0_millis;
|
||||
SREG = oldSREG;
|
||||
|
||||
return m;
|
||||
}
|
||||
|
||||
void delay(unsigned long ms)
|
||||
{
|
||||
unsigned long start = millis();
|
||||
|
||||
while (millis() - start <= ms)
|
||||
;
|
||||
}
|
||||
|
||||
/* Delay for the given number of microseconds. Assumes a 8 or 16 MHz clock.
|
||||
* Disables interrupts, which will disrupt the millis() function if used
|
||||
* too frequently. */
|
||||
void delayMicroseconds(unsigned int us)
|
||||
{
|
||||
uint8_t oldSREG;
|
||||
|
||||
// calling avrlib's delay_us() function with low values (e.g. 1 or
|
||||
// 2 microseconds) gives delays longer than desired.
|
||||
//delay_us(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
|
||||
|
||||
// disable interrupts, otherwise the timer 0 overflow interrupt that
|
||||
// tracks milliseconds will make us delay longer than we want.
|
||||
oldSREG = SREG;
|
||||
cli();
|
||||
|
||||
// busy wait
|
||||
__asm__ __volatile__ (
|
||||
"1: sbiw %0,1" "\n\t" // 2 cycles
|
||||
"brne 1b" : "=w" (us) : "0" (us) // 2 cycles
|
||||
);
|
||||
|
||||
// reenable interrupts.
|
||||
SREG = oldSREG;
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
// this needs to be called before setup() or some functions won't
|
||||
// work there
|
||||
sei();
|
||||
|
||||
// on the ATmega168, timer 0 is also used for fast hardware pwm
|
||||
// (using phase-correct PWM would mean that timer 0 overflowed half as often
|
||||
// resulting in different millis() behavior on the ATmega8 and ATmega168)
|
||||
sbi(TCCR0A, WGM01);
|
||||
sbi(TCCR0A, WGM00);
|
||||
|
||||
// set timer 0 prescale factor to 64
|
||||
sbi(TCCR0B, CS01);
|
||||
sbi(TCCR0B, CS00);
|
||||
|
||||
// enable timer 0 overflow interrupt
|
||||
sbi(TIMSK0, TOIE0);
|
||||
|
||||
// timers 1 and 2 are used for phase-correct hardware pwm
|
||||
// this is better for motors as it ensures an even waveform
|
||||
// note, however, that fast pwm mode can achieve a frequency of up
|
||||
// 8 MHz (with a 16 MHz clock) at 50% duty cycle
|
||||
#if 0
|
||||
// set timer 1 prescale factor to 64
|
||||
sbi(TCCR1B, CS11);
|
||||
sbi(TCCR1B, CS10);
|
||||
|
||||
// put timer 1 in 8-bit phase correct pwm mode
|
||||
sbi(TCCR1A, WGM10);
|
||||
|
||||
// set timer 2 prescale factor to 64
|
||||
sbi(TCCR2B, CS22);
|
||||
|
||||
// configure timer 2 for phase correct pwm (8-bit)
|
||||
sbi(TCCR2A, WGM20);
|
||||
|
||||
// set a2d prescale factor to 128
|
||||
// 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range.
|
||||
// XXX: this will not work properly for other clock speeds, and
|
||||
// this code should use F_CPU to determine the prescale factor.
|
||||
sbi(ADCSRA, ADPS2);
|
||||
sbi(ADCSRA, ADPS1);
|
||||
sbi(ADCSRA, ADPS0);
|
||||
|
||||
// enable a2d conversions
|
||||
sbi(ADCSRA, ADEN);
|
||||
|
||||
// the bootloader connects pins 0 and 1 to the USART; disconnect them
|
||||
// here so they can be used as normal digital i/o; they will be
|
||||
// reconnected in Serial.begin()
|
||||
UCSR0B = 0;
|
||||
#if defined(__AVR_ATmega644P__)
|
||||
//TODO: test to see if disabling this helps?
|
||||
//UCSR1B = 0;
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
@ -1,139 +0,0 @@
|
||||
/*
|
||||
wiring_serial.c - serial functions.
|
||||
Part of Arduino - http://www.arduino.cc/
|
||||
|
||||
Copyright (c) 2005-2006 David A. Mellis
|
||||
Modified 29 January 2009, Marius Kintel for Sanguino - http://www.sanguino.cc/
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General
|
||||
Public License along with this library; if not, write to the
|
||||
Free Software Foundation, Inc., 59 Temple Place, Suite 330,
|
||||
Boston, MA 02111-1307 USA
|
||||
|
||||
$Id: wiring.c 248 2007-02-03 15:36:30Z mellis $
|
||||
*/
|
||||
|
||||
|
||||
#include "wiring_private.h"
|
||||
|
||||
// Define constants and variables for buffering incoming serial data. We're
|
||||
// using a ring buffer (I think), in which rx_buffer_head is the index of the
|
||||
// location to which to write the next incoming character and rx_buffer_tail
|
||||
// is the index of the location from which to read.
|
||||
#define RX_BUFFER_SIZE 128
|
||||
#define RX_BUFFER_MASK 0x7f
|
||||
|
||||
#if defined(__AVR_ATmega644P__)
|
||||
unsigned char rx_buffer[2][RX_BUFFER_SIZE];
|
||||
int rx_buffer_head[2] = {0, 0};
|
||||
int rx_buffer_tail[2] = {0, 0};
|
||||
#else
|
||||
unsigned char rx_buffer[1][RX_BUFFER_SIZE];
|
||||
int rx_buffer_head[1] = {0};
|
||||
int rx_buffer_tail[1] = {0};
|
||||
#endif
|
||||
|
||||
|
||||
#define BEGIN_SERIAL(uart_, baud_) \
|
||||
{ \
|
||||
UBRR##uart_##H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8; \
|
||||
UBRR##uart_##L = ((F_CPU / 16 + baud / 2) / baud - 1); \
|
||||
\
|
||||
/* reset config for UART */ \
|
||||
UCSR##uart_##A = 0; \
|
||||
UCSR##uart_##B = 0; \
|
||||
UCSR##uart_##C = 0; \
|
||||
\
|
||||
/* enable rx and tx */ \
|
||||
sbi(UCSR##uart_##B, RXEN##uart_);\
|
||||
sbi(UCSR##uart_##B, TXEN##uart_);\
|
||||
\
|
||||
/* enable interrupt on complete reception of a byte */ \
|
||||
sbi(UCSR##uart_##B, RXCIE##uart_); \
|
||||
UCSR##uart_##C = _BV(UCSZ##uart_##1)|_BV(UCSZ##uart_##0); \
|
||||
/* defaults to 8-bit, no parity, 1 stop bit */ \
|
||||
}
|
||||
|
||||
void beginSerial(uint8_t uart, long baud)
|
||||
{
|
||||
if (uart == 0) BEGIN_SERIAL(0, baud)
|
||||
#if defined(__AVR_ATmega644P__)
|
||||
else BEGIN_SERIAL(1, baud)
|
||||
#endif
|
||||
}
|
||||
|
||||
#define SERIAL_WRITE(uart_, c_) \
|
||||
while (!(UCSR##uart_##A & (1 << UDRE##uart_))) \
|
||||
; \
|
||||
UDR##uart_ = c
|
||||
|
||||
void serialWrite(uint8_t uart, unsigned char c)
|
||||
{
|
||||
if (uart == 0) {
|
||||
SERIAL_WRITE(0, c);
|
||||
}
|
||||
#if defined(__AVR_ATmega644P__)
|
||||
else {
|
||||
SERIAL_WRITE(1, c);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
int serialAvailable(uint8_t uart)
|
||||
{
|
||||
return (RX_BUFFER_SIZE + rx_buffer_head[uart] - rx_buffer_tail[uart]) & RX_BUFFER_MASK;
|
||||
}
|
||||
|
||||
int serialRead(uint8_t uart)
|
||||
{
|
||||
// if the head isn't ahead of the tail, we don't have any characters
|
||||
if (rx_buffer_head[uart] == rx_buffer_tail[uart]) {
|
||||
return -1;
|
||||
} else {
|
||||
unsigned char c = rx_buffer[uart][rx_buffer_tail[uart]];
|
||||
rx_buffer_tail[uart] = (rx_buffer_tail[uart] + 1) & RX_BUFFER_MASK;
|
||||
return c;
|
||||
}
|
||||
}
|
||||
|
||||
void serialFlush(uint8_t uart)
|
||||
{
|
||||
// don't reverse this or there may be problems if the RX interrupt
|
||||
// occurs after reading the value of rx_buffer_head but before writing
|
||||
// the value to rx_buffer_tail; the previous value of rx_buffer_head
|
||||
// may be written to rx_buffer_tail, making it appear as if the buffer
|
||||
// were full, not empty.
|
||||
rx_buffer_head[uart] = rx_buffer_tail[uart];
|
||||
}
|
||||
|
||||
#define UART_ISR(uart_) \
|
||||
ISR(USART##uart_##_RX_vect) \
|
||||
{ \
|
||||
unsigned char c = UDR##uart_; \
|
||||
\
|
||||
int i = (rx_buffer_head[uart_] + 1) & RX_BUFFER_MASK; \
|
||||
\
|
||||
/* if we should be storing the received character into the location \
|
||||
just before the tail (meaning that the head would advance to the \
|
||||
current location of the tail), we're about to overflow the buffer \
|
||||
and so we don't write the character or advance the head. */ \
|
||||
if (i != rx_buffer_tail[uart_]) { \
|
||||
rx_buffer[uart_][rx_buffer_head[uart_]] = c; \
|
||||
rx_buffer_head[uart_] = i; \
|
||||
} \
|
||||
}
|
||||
|
||||
UART_ISR(0)
|
||||
#if defined(__AVR_ATmega644P__)
|
||||
UART_ISR(1)
|
||||
#endif
|
||||
Loading…
Reference in New Issue