#include "libs/Config.h"
#include "mbed.h"
#include "libs/nuts_bolts.h"
+#include "libs/SlowTicker.h"
#include "modules/communication/SerialConsole.h"
#include "modules/communication/GcodeDispatch.h"
#include "modules/robot/Stepper.h"
#include "modules/robot/Player.h"
+
+
+
+
// List of callback functions, ordered as their corresponding events
const ModuleCallback kernel_callback_functions[NUMBER_OF_DEFINED_EVENTS] = {
&Module::on_main_loop,
// The kernel is the central point in Smoothie : it stores modules, and handles event calls
Kernel::Kernel(){
-
+
// Config first, because we need the baud_rate setting before we start serial
this->config = new Config();
// Serial second, because the other modules might want to say something
this->add_module( this->config );
this->add_module( this->serial );
+ this->slow_ticker = new SlowTicker();
+
// Core modules
this->gcode_dispatch = new GcodeDispatch();
this->robot = new Robot();
#define KERNEL_H
#include "libs/Module.h"
#include "libs/Config.h"
+#include "libs/SlowTicker.h"
#include "modules/communication/SerialConsole.h"
#include "modules/communication/GcodeDispatch.h"
#include "modules/robot/Planner.h"
Player* player;
int debug;
+ SlowTicker* slow_ticker;
private:
vector<Module*> hooks[NUMBER_OF_DEFINED_EVENTS]; // When a module asks to be called for a specific event ( a hook ), this is where that request is remembered
int size();
int capacity();
int next_block_index(int index);
+ int prev_block_index(int index);
void push_back(kind object);
void pop_front(kind &object);
void get( int index, kind &object);
return(index);
}
+template<class kind, int length> int RingBuffer<kind, length>::prev_block_index(int index){
+ if (index == 0) { index = length; }
+ index--;
+ return(index);
+}
+
template<class kind, int length> void RingBuffer<kind, length>::push_back(kind object){
this->buffer[this->tail] = object;
this->tail = (tail+1)&(length-1);
--- /dev/null
+using namespace std;
+#include <vector>
+#include "mbed.h"
+#include "libs/nuts_bolts.h"
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include "SlowTicker.h"
+
+
+SlowTicker* global_slow_ticker;
+
+SlowTicker::SlowTicker(){
+ global_slow_ticker = this;
+ LPC_SC->PCONP |= (1 << 22);
+ LPC_TIM2->MR0 = 1000000;
+ LPC_TIM2->MCR = 3;
+ LPC_TIM2->TCR = 1;
+ NVIC_EnableIRQ(TIMER2_IRQn);
+}
+
+void SlowTicker::set_frequency( int frequency ){
+ LPC_TIM2->MR0 = int(floor((SystemCoreClock/4)/frequency));
+ LPC_TIM2->TCR = 3;
+ LPC_TIM2->TCR = 1;
+}
+
+void SlowTicker::tick(){
+ for (int i=0; i<this->hooks.size(); i++){
+ this->hooks.at(i)->call();
+ }
+}
+
+extern "C" void TIMER2_IRQHandler (void){
+ if((LPC_TIM2->IR >> 0) & 1){
+ LPC_TIM2->IR |= 1 << 0;
+ global_slow_ticker->tick();
+ }
+}
+
--- /dev/null
+#ifndef SLOWTICKER_H
+#define SLOWTICKER_H
+
+using namespace std;
+#include <vector>
+#include "mbed.h"
+#include "libs/nuts_bolts.h"
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+
+class SlowTicker{
+ public:
+ SlowTicker();
+ void set_frequency( int frequency );
+ void tick();
+ // For some reason this can't go in the .cpp, see : http://mbed.org/forum/mbed/topic/2774/?page=1#comment-14221
+ template<typename T> void attach( T *optr, void ( T::*fptr )( void ) ){
+ FunctionPointer* hook = new FunctionPointer();
+ hook->attach(optr, fptr);
+ this->hooks.push_back(hook);
+ }
+
+ vector<FunctionPointer*> hooks;
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif
-/*
- This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
- Smoothie 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.
- Smoothie 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 Smoothie. If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include "mbed.h"
-#include "libs/Kernel.h"
-#include "modules/tools/laser/Laser.h"
-#include "modules/tools/extruder/Extruder.h"
-#include "modules/robot/Player.h"
-#include "modules/utils/simpleshell/SimpleShell.h"
-#include "modules/utils/pauser/Pauser.h"
-#include "libs/SDFileSystem.h"
-#include "libs/Config.h"
-#include "libs/nuts_bolts.h"
-#include "libs/utils.h"
-
-SDFileSystem sd(p5, p6, p7, p8, "sd");
-//LocalFileSystem local("local");
-
-#include <math.h>
-#define UNDEFINED -1
-class TemperatureControl : public Module {
- public:
- TemperatureControl(){
- this->error_count = 0;
- }
-
- void on_module_loaded(){
-
- // We start now desiring any temp
- this->desired_adc_value = UNDEFINED;
-
- // Settings
- this->readings_per_second = 5;
-
- this->r0 = 100000; // Stated resistance eg. 100K
- this->t0 = 25 + 273.15; // Temperature at stated resistance, eg. 25C
- this->beta = 4066; // Thermistor beta rating. See http://reprap.org/bin/view/Main/MeasuringThermistorBeta
- this->vadc = 3.3; // ADC Reference
- this->vcc = 3.3; // Supply voltage to potential divider
- this->k = this->r0 * exp( -this->beta / this->t0 );
- double r1 = 0;
- double r2 = 4700;
-
- if( r1 > 0 ){
- this->vs = r1 * this->vcc / ( r1 + r2 );
- this->rs = r1 * r2 / ( r1 + r2 );
- }else{
- this->vs = this->vcc;
- this->rs = r2;
- }
-
- this->acceleration_factor = 10;
-
- // Setup pins and timer
- this->thermistor_pin = new AnalogIn(p20);
- this->thermistor_read_ticker = new Ticker();
- this->thermistor_read_ticker->attach(this, &TemperatureControl::thermistor_read_tick, 1/this->readings_per_second);
- this->heater_pwm = new PwmOut(p22);
- this->heater_pwm->write(0);
- this->pwm_value = 0;
-
- // Register for events
- this->register_for_event(ON_GCODE_EXECUTE);
-
- }
-
- void on_gcode_execute(void* argument){
- Gcode* gcode = static_cast<Gcode*>(argument);
-
- // Set temperature
- if( gcode->has_letter('M') && gcode->get_value('M') == 104 && gcode->has_letter('S') ){
- this->set_desired_temperature(gcode->get_value('S'));
- }
-
- // Get temperature
- if( gcode->has_letter('M') && gcode->get_value('M') == 105 ){
- this->kernel->serial->printf("get temperature: %f \r\n", this->get_temperature() );
- }
- }
-
- void set_desired_temperature(double desired_temperature){
- this->desired_adc_value = this->temperature_to_adc_value(desired_temperature);
- this->tail_adc_value = this->temperature_to_adc_value(desired_temperature-20);
- this->head_adc_value = this->temperature_to_adc_value(desired_temperature+5);
- //this->kernel->serial->printf("requested temperature: %f, adc value: %f, k: %f \r\n", desired_temperature, this->desired_adc_value, this->k );
- }
-
- double get_temperature(){
- double temp = this->new_thermistor_reading() ;
- //this->kernel->serial->printf("adc reading: %f \r\n", temp);
- return this->adc_value_to_temperature( this->new_thermistor_reading() );
- }
-
- double adc_value_to_temperature(double adc_value){
- double v = adc_value * this->vadc; // Convert from 0-1 adc value to voltage
- double r = this->rs * v / ( this->vs - v ); // Resistance of thermistor
- return ( this->beta / log( r / this->k )) - 273.15;
- }
-
- double temperature_to_adc_value(double temperature){
- double r = this->r0 * exp( this->beta * ( 1 / (temperature + 273.15) -1 / this->t0 ) ); // Resistance of the thermistor
- double v = this->vs * r / ( this->rs + r ); // Voltage at the potential divider
- return v / this->vadc * 1.00000; // The ADC reading
- }
-
- void thermistor_read_tick(){
- double reading = this->new_thermistor_reading();
- if( this->desired_adc_value != UNDEFINED ){
- double difference = fabs( reading - this->desired_adc_value );
- double adjustment = difference / acceleration_factor / this->readings_per_second;
- if( reading > this->tail_adc_value ){
- this->heater_pwm->write( 1 );
- //this->kernel->serial->printf("under: %f \r\n", this->adc_value_to_temperature(reading) );
- }else if( reading < this->head_adc_value ){
- this->pwm_value -= adjustment;
- this->heater_pwm->write( 0 );
- //this->kernel->serial->printf("over: %f \r\n", this->adc_value_to_temperature(reading) );
- }else{
- if( reading > this->desired_adc_value ){
- this->pwm_value += adjustment; // Heat up
- }else{
- this->pwm_value -= adjustment; // Heat down
- }
- this->pwm_value = max( double(0), min( double(1), pwm_value ) );
- this->heater_pwm->write( pwm_value );
- //this->kernel->serial->printf("temp: %f \r\n", this->adc_value_to_temperature(reading) );
- //this->kernel->serial->printf("read:%.5f, des_adc_val:%.5ff, diff: %.5f, ad: %f, pwm:%f \r\n", reading, this->desired_adc_value, difference, adjustment, pwm_value );
- }
- }
- }
-
- double new_thermistor_reading(){
- double new_reading = this->thermistor_pin->read();
-
- if( this->queue.size() < 15 ){
- this->queue.push_back( new_reading );
- //this->kernel->serial->printf("first\r\n");
- return new_reading;
- }else{
- double current_temp = this->average_adc_reading();
- double error = fabs(new_reading - current_temp);
- if( error < 0.1 ){
- this->error_count = 0;
- double test;
- this->queue.pop_front(test);
- this->queue.push_back( new_reading );
- }else{
- this->error_count++;
- if( this->error_count > 4 ){
- double test;
- this->queue.pop_front(test);
- }
- }
- return current_temp;
- //this->kernel->serial->printf("read: %f temp: %f average: %f error: %f queue: %d \r\n",this->adc_value_to_temperature( new_reading ), this->adc_value_to_temperature(current_temp), current_temp, error, this->queue.size() );
- }
-
-
-
- }
-
- double average_adc_reading(){
- double total;
- int j=0;
- int reading_index = this->queue.head;
- //this->kernel->serial->printf("[");
- //for( int i = 0; i <= this->queue.size()-1; i++ ){
- while( reading_index != this->queue.tail ){
- j++;
- //this->kernel->serial->printf("value:%f\r\n", *(this->queue.get_ref(i)) );
- total += this->queue.buffer[reading_index];
- //this->kernel->serial->printf("%.4f,", this->queue.buffer[reading_index]);
- reading_index = this->queue.next_block_index( reading_index );
- }
- //this->kernel->serial->printf("]->[%4f=%4f]\r\n", total/j, this->adc_value_to_temperature(total/j));
- //this->kernel->serial->printf("total:%f, size:%d \r\n", total, this->queue.size() );
- return total / j;
- }
-
-// double average_adc_reading(){
-// double total;
-// int j=0;
-// for( int i = 0; i <= this->queue.size()-1; i++ ){
-// j++;
-// //this->kernel->serial->printf("value:%f\r\n", *(this->queue.get_ref(i)) );
-// total += *(this->queue.get_ref(i));
-// }
-// //this->kernel->serial->printf("total:%f, size:%d \r\n", total, this->queue.size() );
-// return total / j;
-// }
-
- AnalogIn* thermistor_pin;
- Ticker* thermistor_read_ticker;
- Ticker* debug_ticker;
- PwmOut* heater_pwm;
- double pwm_value;
- double desired_adc_value;
- double tail_adc_value;
- double head_adc_value;
-
- // Thermistor computation settings
- double r0;
- double t0;
- double beta;
- double vadc;
- double vcc;
- double k;
- double vs;
- double rs;
-
- double acceleration_factor;
- double readings_per_second;
-
- RingBuffer<double,16> queue; // Queue of Blocks
- int error_count;
-};
-
-
-int main() {
-
- Kernel* kernel = new Kernel();
-
- kernel->serial->printf("Smoothie ( grbl port ) version 0.4 \r\nstart\r\n");
-
- kernel->add_module( new Laser(p21) );
- kernel->add_module( new Extruder(p26,p27) );
- kernel->add_module( new SimpleShell() );
- //kernel->add_module( new Pauser(p29,p30) );
- kernel->add_module( new TemperatureControl() );
-
- while(1){
- kernel->call_event(ON_MAIN_LOOP);
-
- }
-}
-
+/*
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ Smoothie 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.
+ Smoothie 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 Smoothie. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "mbed.h"
+#include "libs/Kernel.h"
+#include "modules/tools/laser/Laser.h"
+#include "modules/tools/extruder/Extruder.h"
+#include "modules/tools/temperaturecontrol/TemperatureControl.h"
+#include "modules/robot/Player.h"
+#include "modules/utils/simpleshell/SimpleShell.h"
+#include "modules/utils/pauser/Pauser.h"
+#include "libs/SDFileSystem.h"
+#include "libs/Config.h"
+#include "libs/nuts_bolts.h"
+#include "libs/utils.h"
+
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+//LocalFileSystem local("local");
+
+int main() {
+
+ Kernel* kernel = new Kernel();
+
+ kernel->serial->printf("Smoothie ( grbl port ) version 0.4 \r\nstart\r\n");
+
+ kernel->add_module( new Laser(p21) );
+ kernel->add_module( new Extruder(p26,p27) );
+ kernel->add_module( new SimpleShell() );
+ //kernel->add_module( new Pauser(p29,p30) );
+ kernel->add_module( new TemperatureControl() );
+
+ while(1){
+ kernel->call_event(ON_MAIN_LOOP);
+
+ }
+}
+
// Do not append block with no movement
//if( target[ALPHA_STEPPER] == this->position[ALPHA_STEPPER] && target[BETA_STEPPER] == this->position[BETA_STEPPER] && target[GAMMA_STEPPER] == this->position[GAMMA_STEPPER] ){ this->computing = false; return; }
+
+
// Stall here if the queue is ful
- while( this->kernel->player->queue.size() >= this->kernel->player->queue.capacity()-2 ){ wait_us(100); }
+ //this->kernel->serial->printf("aaa\r\n");
+ while( this->kernel->player->queue.size() >= this->kernel->player->queue.capacity()-2 ){
+ wait_us(500);
+ }
+ //this->kernel->serial->printf("bbb\r\n");
Block* block = this->kernel->player->new_block();
block->planner = this;
// 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(){
- // For each block
- for( int index = this->kernel->player->queue.size()-1; index > 0; index-- ){ // Skip buffer tail/first block to prevent over-writing the initial entry speed.
- this->kernel->player->queue.get_ref(index)->reverse_pass((index==this->kernel->player->queue.size()-1?NULL:this->kernel->player->queue.get_ref(index+1)), (index==0? (this->has_deleted_block?&(this->last_deleted_block):NULL) :this->kernel->player->queue.get_ref(index-1)));
+ // For each block
+ int block_index = this->kernel->player->queue.tail;
+ Block* blocks[3] = {NULL,NULL,NULL};
+
+ while(block_index!=this->kernel->player->queue.head){
+ block_index = this->kernel->player->queue.prev_block_index( block_index );
+ blocks[2] = blocks[1];
+ blocks[1] = blocks[0];
+ blocks[0] = &this->kernel->player->queue.buffer[block_index];
+ if( blocks[1] == NULL ){ continue; }
+ blocks[1]->reverse_pass(blocks[2], blocks[0]);
}
+
+
+
+
+ //for( int index = this->kernel->player->queue.size()-1; index > 0; index-- ){ // Skip buffer tail/first block to prevent over-writing the initial entry speed.
+ // this->kernel->player->queue.get_ref(index)->reverse_pass((index==this->kernel->player->queue.size()-1?NULL:this->kernel->player->queue.get_ref(index+1)), (index==0? (this->has_deleted_block?&(this->last_deleted_block):NULL) :this->kernel->player->queue.get_ref(index-1)));
+ //}
}
// 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() {
// For each block
- for( int index = 0; index <= this->kernel->player->queue.size()-1; index++ ){
- this->kernel->player->queue.get_ref(index)->forward_pass((index==0?NULL:this->kernel->player->queue.get_ref(index-1)),(index==this->kernel->player->queue.size()-1?NULL:this->kernel->player->queue.get_ref(index+1)));
- }
+ int block_index = this->kernel->player->queue.head;
+ Block* blocks[3] = {NULL,NULL,NULL};
+
+ while(block_index!=this->kernel->player->queue.tail){
+ blocks[0] = blocks[1];
+ blocks[1] = blocks[2];
+ blocks[2] = &this->kernel->player->queue.buffer[block_index];
+ if( blocks[0] == NULL ){ continue; }
+ blocks[1]->forward_pass(blocks[0],blocks[2]);
+ block_index = this->kernel->player->queue.next_block_index( block_index );
+ }
+ blocks[2]->forward_pass(blocks[1],NULL);
+
+ //for( int index = 0; index <= this->kernel->player->queue.size()-1; index++ ){
+ // this->kernel->player->queue.get_ref(index)->forward_pass((index==0?NULL:this->kernel->player->queue.get_ref(index-1)),(index==this->kernel->player->queue.size()-1?NULL:this->kernel->player->queue.get_ref(index+1)));
+ //}
}
// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
// compute the two adjacent trapezoids to the junction, since the junction speed corresponds
// to exit speed and entry speed of one another.
-/*
-void Planner::recalculate_trapezoids() {
- // For each block
- int size = this->kernel->player->queue.size();
- for( int index = 0; index <= size-1; index++ ){ // We skip the first one because we need a previous
- if( size-1 == index ){ //last block
- Block* last = this->kernel->player->queue.get_ref(index);
- last->calculate_trapezoid( last->entry_speed / last->nominal_speed, MINIMUM_PLANNER_SPEED / last->nominal_speed );
- this->kernel->serial->printf("%p: %d/%d last r:%d \r\n", last, index, size-1, last->initial_rate);
- }else{
- Block* current = this->kernel->player->queue.get_ref(index);
- Block* next = this->kernel->player->queue.get_ref(index+1);
- if( current->recalculate_flag || next->recalculate_flag ){
- current->calculate_trapezoid( current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed );
- current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
- this->kernel->serial->printf("%p: %d/%d other r:%d \r\n", current, index, size-1, current->initial_rate);
- }else{
- this->kernel->serial->printf("%p: %d/%d else r:%d \r\n", current, index, size-1, current->initial_rate);
- }
- }
- }
-}
-*/
void Planner::recalculate_trapezoids() {
int block_index = this->kernel->player->queue.head;
Block* current;
break;
}
-
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
using namespace std;
#include "libs/nuts_bolts.h"
-Timeout flipper;
Stepper* stepper;
Stepper::Stepper(){
// Get onfiguration
this->on_config_reload(this);
- // Acceleration timer
- this->acceleration_ticker.attach_us(this, &Stepper::trapezoid_generator_tick, 1000000/this->acceleration_ticks_per_second);
+ // Acceleration ticker
+ this->kernel->slow_ticker->set_frequency(this->acceleration_ticks_per_second);
+ this->kernel->slow_ticker->attach( this, &Stepper::trapezoid_generator_tick );
// Initiate main_interrupt timer and step reset timer
LPC_TIM0->MR0 = 10000;
LPC_TIM0->MCR = 11; // for MR0 and MR1, with no reset at MR1
NVIC_EnableIRQ(TIMER0_IRQn);
- NVIC_SetPriority(TIMER3_IRQn, 3);
- NVIC_SetPriority(TIMER0_IRQn, 2);
- NVIC_SetPriority(TIMER1_IRQn, 2);
+ NVIC_SetPriority(TIMER3_IRQn, 4);
+ NVIC_SetPriority(TIMER0_IRQn, 1);
+ NVIC_SetPriority(TIMER2_IRQn, 2);
+ NVIC_SetPriority(TIMER1_IRQn, 3);
LPC_TIM0->TCR = 1;
-
+
+
// Step and Dir pins as outputs
this->step_gpio_port->FIODIR |= this->step_mask;
this->dir_gpio_port->FIODIR |= this->dir_mask;
// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
void Stepper::on_pause(void* argument){
LPC_TIM0->TCR = 0;
- this->acceleration_ticker.detach();
+ LPC_TIM2->TCR = 0;
}
// When the play/pause button is set to play, or a module calls the ON_PLAY event
void Stepper::on_play(void* argument){
LPC_TIM0->TCR = 1;
- this->acceleration_ticker.attach_us(this, &Stepper::trapezoid_generator_tick, 1000000/this->acceleration_ticks_per_second);
+ LPC_TIM2->TCR = 1;
}
// A new block is popped from the queue
// The stepper does not care about 0-blocks
if( block->millimeters == 0.0 ){ return; }
- //this->current_block = block;
-
// Mark the new block as of interrest to us
block->take();
-
+
// Setup
for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
this->step_events_completed = 0;
this->current_block = block;
-
+
+ // This is just to save computing power and not do it every step
this->update_offsets();
+
+ // Setup acceleration for this block
this->trapezoid_generator_reset();
+
}
// Current block is discarded
void Stepper::on_block_end(void* argument){
Block* block = static_cast<Block*>(argument);
+ LPC_TIM0->MR0 = 1000000;
this->current_block = NULL; //stfu !
}
}
}
-
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Smoothie. It is executed at the rate set with
// config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
inline void Stepper::main_interrupt(){
this->step_gpio_port->FIOSET = ( this->out_bits ^ this->step_invert_mask ) & this->step_mask;
this->step_gpio_port->FIOCLR = ( ~ ( this->out_bits ^ this->step_invert_mask ) ) & this->step_mask;
-
if( this->current_block != NULL ){
// Set bits for direction and steps
this->out_bits = this->current_block->direction_bits;
}
}
}
-
}else{
this->out_bits = 0;
}
}
}
-
// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
// interrupt. It can be assumed that the trapezoid-generator-parameters and the
// current_block stays untouched by outside handlers for the duration of this function call.
void Stepper::trapezoid_generator_tick() {
- if(this->current_block ) {
- if(this->step_events_completed < this->current_block->accelerate_until<<16) {
- this->trapezoid_adjusted_rate += this->current_block->rate_delta;
- if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
- this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
- }
- this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
- } else if (this->step_events_completed > this->current_block->decelerate_after<<16) {
- // NOTE: We will only reduce speed if the result will be > 0. This catches small
- // rounding errors that might leave steps hanging after the last trapezoid tick.
- if (this->trapezoid_adjusted_rate > this->current_block->rate_delta) {
- this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
- }
- if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
- this->trapezoid_adjusted_rate = this->current_block->final_rate;
- }
- this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
- } else {
- // Make sure we cruise at exactly nominal rate
- if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
- this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
+ if(this->current_block && !this->trapezoid_generator_busy ) {
+ if(this->step_events_completed < this->current_block->accelerate_until<<16) {
+ this->trapezoid_adjusted_rate += this->current_block->rate_delta;
+ if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
+ this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
+ }
+ this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+ } else if (this->step_events_completed >= this->current_block->decelerate_after<<16) {
+ // NOTE: We will only reduce speed if the result will be > 0. This catches small
+ // rounding errors that might leave steps hanging after the last trapezoid tick.
+ if (this->trapezoid_adjusted_rate > double(this->current_block->rate_delta) * 1.5) {
+ this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
+ }else{
+ this->trapezoid_adjusted_rate = floor(double(this->trapezoid_adjusted_rate / 2 ));
+ }
+ if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
+ this->trapezoid_adjusted_rate = this->current_block->final_rate;
+ }
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+ } else {
+ // Make sure we cruise at exactly nominal rate
+ if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
+ this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
+ this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+ }
}
- }
- }
+ }
}
-
// Initializes the trapezoid generator from the current block. Called whenever a new
// block begins.
void Stepper::trapezoid_generator_reset(){
this->trapezoid_adjusted_rate = this->current_block->initial_rate;
this->trapezoid_tick_cycle_counter = 0;
+ // Because this can be called directly from the main loop, it could be interrupted by the acceleration ticker, and that would be bad, so we use a flag
+ this->trapezoid_generator_busy = true;
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
this->trapezoid_generator_busy = false;
}
// Set the Timer interval
LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * speed_factor ) );
+
if( LPC_TIM0->MR0 < 150 ){
LPC_TIM0->MR0 = 150;
-
this->kernel->serial->printf("tim0mr0: %d, steps_per minute: %f \r\n", LPC_TIM0->MR0, steps_per_minute );
- //Block* block = new Block();
- //this->kernel->serial->printf(":l queue:%d debug:%d cb:%p cb=null:%d mem:%p tc:%u mr0:%u mr1:%u \r\n", this->kernel->player->queue.size() , this->kernel->debug, this->current_block, this->current_block == NULL, block, LPC_TIM0->TC , LPC_TIM0->MR0, LPC_TIM0->MR1 );
- //delete block;
- //this->kernel->serial->printf("mr0:%u, st/m:%f, sf:%f, bsf:%f \r\n", LPC_TIM0->MR0, steps_per_minute, speed_factor, this->base_stepping_frequency );
- //this->current_block->debug(this->kernel);
- //wait(0.1);
}
-
// In case we change the Match Register to a value the Timer Counter has past
if( LPC_TIM0->TC >= LPC_TIM0->MR0 ){ LPC_TIM0->TCR = 3; LPC_TIM0->TCR = 1; }
this->kernel->call_event(ON_SPEED_CHANGE, this);
-
+
}
int config_step_timer( int cycles );
Block* current_block;
- Ticker acceleration_ticker;
- Timeout flipper;
int counters[3];
int stepped[3];
int offsets[3];
#include "modules/tools/extruder/Extruder.h"
-
Extruder* extruder_for_irq; // We need this global because ISRs can't be attached to an object
extern "C" void TIMER1_IRQHandler (void){
if((LPC_TIM1->IR >> 1) & 1){
Extruder::Extruder(PinName stppin, PinName dirpin) : step_pin(stppin), dir_pin(dirpin) {
this->absolute_mode = true;
this->direction = 1;
+ this->acceleration_lock = false;
}
void Extruder::on_module_loaded() {
- this->debug = false;
-
+ // Do not do anything if not enabledd
if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
+ // Because we can't make an irq handler an method directly
extruder_for_irq = this;
// Settings
this->register_for_event(ON_BLOCK_BEGIN);
this->register_for_event(ON_BLOCK_END);
this->register_for_event(ON_GCODE_EXECUTE);
- this->register_for_event(ON_SPEED_CHANGE);
+ // Start values
this->start_position = 0;
this->target_position = 0;
this->current_position = 0;
this->current_block = NULL;
-
+ this->mode = OFF;
+
// Start Timer1
- LPC_TIM1->MR0 = 10000;
+ // TODO: Use the same timer as the stepper
+ LPC_TIM1->MR0 = 1000000;
LPC_TIM1->MR1 = 500;
LPC_TIM1->MCR = 11; // For MR0 and MR1, with no reset at MR1
NVIC_EnableIRQ(TIMER1_IRQn);
- LPC_TIM1->TCR = 1;
+ LPC_TIM1->TCR = 1;
+
+ // Update speed every *acceleration_ticks_per_second*
+ // TODO: Make this an independent setting
+ this->kernel->slow_ticker->attach( this, &Extruder::acceleration_tick );
+
}
// Get config
this->steps_per_millimeter = this->kernel->config->value(steps_per_millimeter_checksum )->by_default(1)->as_number();
this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(1)->as_number();
this->acceleration = this->kernel->config->value(acceleration_checksum )->by_default(1)->as_number();
+ LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
}
-// Computer extrusion speed based on parameters and gcode distance of travel
+// Compute extrusion speed based on parameters and gcode distance of travel
void Extruder::on_gcode_execute(void* argument){
Gcode* gcode = static_cast<Gcode*>(argument);
-
- this->kernel->serial->printf("e: %s\r\n", gcode->command.c_str() );
-
+
// Absolute/relative mode
if( gcode->has_letter('M')){
int code = gcode->get_value('M');
if( code == 82 ){ this->absolute_mode == true; }
if( code == 83 ){ this->absolute_mode == false; }
}
-
+
+ // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude
+ this->mode = OFF;
+
if( gcode->has_letter('G') ){
-
+ // G92: Reset extruder position
if( gcode->get_value('G') == 92 ){
-
if( gcode->has_letter('E') ){
this->current_position = gcode->get_value('E');
this->target_position = this->current_position;
this->start_position = this->current_position;
}
-
}else{
-
- // Extrusion length
+ // Extrusion length from 'G' Gcode
if( gcode->has_letter('E' )){
- double extrusion_distance = gcode->get_value('E');
- //this->kernel->serial->printf("a extrusion_distance: %f, target_position: %f, new_extrusion_distance: %f, current_position: %f \r\n", extrusion_distance, this->target_position, extrusion_distance - this->target_position, this->current_position );
- if( this->absolute_mode == true ){
- extrusion_distance = extrusion_distance - this->target_position;
- }
- //this->kernel->serial->printf("b extrusion_distance: %f, target_position: %f \r\n", extrusion_distance, this->target_position );
- if( fabs(gcode->millimeters_of_travel) < 0.0001 ){
- this->solo_mode = true;
- this->travel_distance = extrusion_distance;
- this->travel_ratio = 0.0;
- if( gcode->has_letter('F') ){
- this->feed_rate = gcode->get_value('F');
- }
+ // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position )
+ double relative_extrusion_distance = gcode->get_value('E');
+ if( this->absolute_mode == true ){ relative_extrusion_distance = relative_extrusion_distance - this->target_position; }
+
+ // If the robot is moving, we follow it's movement, otherwise, we move alone
+ if( fabs(gcode->millimeters_of_travel) < 0.0001 ){ // With floating numbers, we can have 0 != 0 ... beeeh
+ this->mode = SOLO;
+ this->travel_distance = relative_extrusion_distance;
+ if( gcode->has_letter('F') ){ this->feed_rate = gcode->get_value('F'); }
}else{
- this->solo_mode = false;
- this->travel_ratio = extrusion_distance / gcode->millimeters_of_travel;
- this->travel_distance = 0.0;
+ this->mode = FOLLOW;
+ // We move proportionally to the robot's movement
+ this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel;
}
- // Else do not extrude
- }else{
- this->travel_ratio = 0.0;
- this->travel_distance = 0.0;
- }
-
+ }
}
-
}
-
+
}
-
-
+// When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing )
void Extruder::on_block_begin(void* argument){
Block* block = static_cast<Block*>(argument);
- if( fabs(this->travel_distance) > 0.001 ){
- block->take(); // In solo mode we take the block so we can move even if the stepper has nothing to do
+ if( this->mode == SOLO ){
+ // In solo mode we take the block so we can move even if the stepper has nothing to do
+ block->take();
+ LPC_TIM1->TCR = 1;
this->current_block = block;
this->start_position = this->target_position;
this->target_position = this->start_position + this->travel_distance ;
- this->kernel->serial->printf("bb: target_position: %f, travel_distance:%f, current_position:%f \r\n", this->target_position, this->travel_distance, this->current_position );
- this->on_speed_change(this);
- }
- if( fabs(this->travel_ratio) > 0.001 ){
+ this->travel_ratio = 0.2; // TODO : Make a real acceleration thing
+ if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
+ this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second
+ }else if( this->mode == FOLLOW ){
// In non-solo mode, we just follow the stepper module
+ LPC_TIM1->TCR = 1;
this->current_block = block;
this->start_position = this->target_position;
this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio );
- this->kernel->serial->printf("bb: target_position: %f, travel_ratio:%f, current_position:%f \r\n", this->target_position, this->travel_ratio, this->current_position );
- this->on_speed_change(this);
+ if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; }
+ this->acceleration_tick();
}
}
+// When a block ends, pause the stepping interrupt
void Extruder::on_block_end(void* argument){
Block* block = static_cast<Block*>(argument);
- this->kernel->serial->printf("cc: target_position: %f, travel_ratio:%f, travel_distance:%f, current_position:%f \r\n", this->target_position, this->travel_ratio, this->travel_distance, this->current_position );
+ LPC_TIM1->MR0 = 1000000;
+ LPC_TIM1->TCR = 0;
this->current_block = NULL;
}
-void Extruder::on_speed_change(void* argument){
+// Called periodically to change the speed to match acceleration or to match the speed of the robot
+void Extruder::acceleration_tick(){
- // Set direction
- if( this->target_position > this->current_position ){
- this->direction = 1;
- }else if( this->target_position < this->current_position ){
- this->direction = -1;
- }
-
- if( fabs(this->travel_ratio) > 0.001 ){
-
- if( this->kernel->stepper->current_block == NULL || fabs(this->kernel->stepper->trapezoid_adjusted_rate) < 0.0001 || this->kernel->stepper->current_block->nominal_rate == 0 ){
+ // Avoid trying to work when we really shouldn't ( between blocks or re-entry )
+ if( this->current_block == NULL || this->acceleration_lock ){ return; }
+ this->acceleration_lock = true;
+
+ // In solo mode, we mode independently from the robot
+ if( this->mode == SOLO ){
+ // TODO : Do real acceleration here
+ this->travel_ratio += 0.03;
+ if( this->travel_ratio > 1 ){ this->travel_ratio = 1; }
+ this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) ); // Speed in steps per second
+
+ // In follow mode we match the speed of the robot, + eventually advance
+ }else if( this->mode == FOLLOW ){
+ Stepper* stepper = this->kernel->stepper; // Just for convenience
+
+ // Strategy :
+ // * Find where in the block will the stepper be at the next tick ( if the block will have ended then, don't change speed )
+ // * Find what position this is for us
+ // * Find what speed we must go at to be at that position for the next acceleration tick
+ // TODO : This works, but PLEASE PLEASE PLEASE if you know a better way to do it, do it better, I don't find this elegant at all, it's just the best I could think of
+
+
+ int ticks_forward = 3;
+ // We need to take those values here, and then use those instead of the live values, because using the live values inside the loop can break things ( infinite loops etc ... )
+ double next_stepper_rate = stepper->trapezoid_adjusted_rate;
+ double step_events_completed = (double(double(stepper->step_events_completed)/double(1<<16)));
+ double position = ( this->current_position - this->start_position ) * this->direction ;
+ double length = fabs( this->start_position - this->target_position );
+ double last_ratio = -1;
+
+ // Do the startegy above, but if it does not work, look a bit further and try again, and again ...
+ while(1){
+
+ // Find the position where we should be at the next tick
+ double next_ratio = double( step_events_completed + ( next_stepper_rate / 60 / ((double(stepper->acceleration_ticks_per_second)/ticks_forward)) ) ) / double( this->current_block->steps_event_count );
+ double next_relative_position = ( length * next_ratio );
+
+ // Advance
+ // TODO: Proper advance configuration
+ double advance = double(next_stepper_rate) * 0.00001 * 0.15;
+ next_relative_position += ( advance );
+
+ // TODO : all of those "if->return" is very hacky, we should do the math in a way where most of those don't happen, but that requires doing tons of drawing ...
+ if( last_ratio == next_ratio ){ this->acceleration_lock = false; return; }else{ last_ratio = next_ratio; }
+ if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return; }
+ if( ticks_forward > 1000 ){ this->acceleration_lock = false; return; } // This is very ugly
+
+ // Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value
+ if( position > next_relative_position ){
+ double far_back = position - next_relative_position;
+ double far_back_ratio = far_back / length;
+ double move_duration = double( this->current_block->steps_event_count ) / ( double(next_stepper_rate) / 60 ) ;
+ double ticks_in_a_move = round( stepper->acceleration_ticks_per_second * move_duration );
+ double ratio_per_tick = 1 / ticks_in_a_move;
+ double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1;
+ ticks_forward += ticks_to_equilibrium;
+ // Because this is a loop, and we can be interrupted by the stepping interrupt, if that interrupt changes block, the new block may not be solo, and we may get trapped into an infinite loop
+ if( this->mode != FOLLOW ){ this->acceleration_lock = false; return; }
+ continue;
+ }
+
+ // Finally, compute the speed to get to that next position
+ double next_absolute_position = this->start_position + ( this->direction * next_relative_position );
+ double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter;
+ double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) );
+
+ // Change stepping speed
+ this->set_speed( speed_to_next_tick );
+
+ this->acceleration_lock = false;
return;
}
-
- // Get a current/nominal rate ratio
- double stepper_rate_ratio = this->kernel->stepper->trapezoid_adjusted_rate / double( this->kernel->stepper->current_block->nominal_rate ) ;
- // Get a nominal duration for this block
- double nominal_duration = this->kernel->stepper->current_block->millimeters / ( this->kernel->stepper->current_block->nominal_speed / 60 ) ;
- // Get extrusion nominal speed
- double nominal_extrusion_speed = fabs( this->target_position - this->start_position ) / nominal_duration;
- // Get adjusted speed
- double adjusted_speed = nominal_extrusion_speed * stepper_rate_ratio;
-
- // Set timer
- if((adjusted_speed*this->steps_per_millimeter) < 1 ){
- return;
- }
- LPC_TIM1->MR0 = ((SystemCoreClock/4))/(adjusted_speed*this->steps_per_millimeter);
- if( LPC_TIM1->MR0 < 300 ){
- //this->kernel->serial->printf("tim1mr0 %d, adjusted_speed: %f\r\n", LPC_TIM1->MR0, adjusted_speed );
- LPC_TIM1->MR0 = 300;
- }
-
- // In case we are trying to set the timer to a limit it has already past by
- if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
- LPC_TIM1->TCR = 3;
- LPC_TIM1->TCR = 1;
- }
-
- // Update Timer1
- LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
-
-
}
- if( fabs(this->travel_distance) > 0.001 ){
-
- // Set timer
- LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor((this->feed_rate/60)*this->steps_per_millimeter));
-
- // In case we are trying to set the timer to a limit it has already past by
- if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){
- LPC_TIM1->TCR = 3;
- LPC_TIM1->TCR = 1;
- }
-
- // Update Timer1
- LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
+ this->acceleration_lock = false;
+}
+// Convenience function to set stepping speed
+void Extruder::set_speed( int steps_per_second ){
+
+ // TODO : Proper limit config value
+ if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){
+ steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60;
}
+ LPC_TIM1->MR0 = (SystemCoreClock/4)/steps_per_second;
+ if( LPC_TIM1->TC >= LPC_TIM1->MR0 ){ // In case we are trying to set the timer to a limit it has already past by
+ LPC_TIM1->TCR = 3;
+ LPC_TIM1->TCR = 1;
+ }
}
-
inline void Extruder::stepping_tick(){
-
- //if( fabs( this->current_position - this->target_position ) >= 0.001 ){
+ // If we still have steps to do
+ // TODO: Step using the same timer as the robot, and count steps instead of absolute float position
if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){
- this->current_position += (double(double(1)/double(1100)))*double(this->direction);
+ this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction);
this->dir_pin = ((this->direction > 0) ? 0 : 1);
this->step_pin = 1;
}else{
// Move finished
- if( fabs(this->travel_distance) > 0.001 && this->current_block != NULL ){
+ if( this->mode == SOLO && this->current_block != NULL ){
+ // In follow mode, the robot takes and releases the block, in solo mode we do
this->current_block->release();
}
}
#define default_feed_rate_checksum 53183
#define acceleration_checksum 60356
+#define OFF 0
+#define SOLO 1
+#define FOLLOW 2
class Extruder : public Module{
public:
void on_gcode_execute(void* argument);
void on_block_begin(void* argument);
void on_block_end(void* argument);
- void on_speed_change(void* argument);
+ void set_speed(int steps_per_second);
void acceleration_tick();
void stepping_tick();
double start_position; // Start point ( in steps ) for the current move
double target_position; // End point ( in steps ) for the current move
double current_position; // Current point ( in steps ) for the current move, incremented every time a step is outputed
- Ticker acceleration_ticker; // Ticker responsible with updating the speed ( acceleration management ). Uses Timer3
Block* current_block; // Current block we are stepping, same as Stepper's one
int microseconds_per_step_pulse; // Pulse duration for step pulses
double steps_per_millimeter; // Steps to travel one millimeter
double feed_rate; //
double acceleration; //
+
bool solo_mode;
double travel_ratio;
double travel_distance;
bool debug;
int debug_count;
+
+ char mode;
+ bool acceleration_lock;
};
#endif
--- /dev/null
+#include "mbed.h"
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include <math.h>
+#include "TemperatureControl.h"
+
+TemperatureControl::TemperatureControl(){
+ this->error_count = 0;
+}
+
+void TemperatureControl::on_module_loaded(){
+
+ // We start now desiring any temp
+ this->desired_adc_value = UNDEFINED;
+
+ // Settings
+ this->readings_per_second = 5;
+
+ this->r0 = 100000; // Stated resistance eg. 100K
+ this->t0 = 25 + 273.15; // Temperature at stated resistance, eg. 25C
+ this->beta = 4066; // Thermistor beta rating. See http://reprap.org/bin/view/Main/MeasuringThermistorBeta
+ this->vadc = 3.3; // ADC Reference
+ this->vcc = 3.3; // Supply voltage to potential divider
+ this->k = this->r0 * exp( -this->beta / this->t0 );
+ double r1 = 0;
+ double r2 = 4700;
+
+ if( r1 > 0 ){
+ this->vs = r1 * this->vcc / ( r1 + r2 );
+ this->rs = r1 * r2 / ( r1 + r2 );
+ }else{
+ this->vs = this->vcc;
+ this->rs = r2;
+ }
+
+ this->acceleration_factor = 10;
+
+ // Setup pins and timer
+ this->thermistor_pin = new AnalogIn(p20);
+ this->kernel->slow_ticker->attach( this, &TemperatureControl::thermistor_read_tick );
+ this->heater_pwm = new PwmOut(p22);
+ this->heater_pwm->write(0);
+ this->pwm_value = 0;
+
+ // Register for events
+ this->register_for_event(ON_GCODE_EXECUTE);
+
+}
+
+void TemperatureControl::on_gcode_execute(void* argument){
+ Gcode* gcode = static_cast<Gcode*>(argument);
+
+ // Set temperature
+ if( gcode->has_letter('M') && gcode->get_value('M') == 104 && gcode->has_letter('S') ){
+ this->set_desired_temperature(gcode->get_value('S'));
+ }
+
+ // Get temperature
+ if( gcode->has_letter('M') && gcode->get_value('M') == 105 ){
+ this->kernel->serial->printf("get temperature: %f \r\n", this->get_temperature() );
+ }
+}
+
+void TemperatureControl::set_desired_temperature(double desired_temperature){
+ this->desired_adc_value = this->temperature_to_adc_value(desired_temperature);
+ this->tail_adc_value = this->temperature_to_adc_value(desired_temperature-20);
+ this->head_adc_value = this->temperature_to_adc_value(desired_temperature+5);
+}
+
+double TemperatureControl::get_temperature(){
+ double temp = this->new_thermistor_reading() ;
+ return this->adc_value_to_temperature( this->new_thermistor_reading() );
+}
+
+double TemperatureControl::adc_value_to_temperature(double adc_value){
+ double v = adc_value * this->vadc; // Convert from 0-1 adc value to voltage
+ double r = this->rs * v / ( this->vs - v ); // Resistance of thermistor
+ return ( this->beta / log( r / this->k )) - 273.15;
+}
+
+double TemperatureControl::temperature_to_adc_value(double temperature){
+ double r = this->r0 * exp( this->beta * ( 1 / (temperature + 273.15) -1 / this->t0 ) ); // Resistance of the thermistor
+ double v = this->vs * r / ( this->rs + r ); // Voltage at the potential divider
+ return v / this->vadc * 1.00000; // The ADC reading
+}
+
+void TemperatureControl::thermistor_read_tick(){
+
+ double reading = this->new_thermistor_reading();
+ if( this->desired_adc_value != UNDEFINED ){
+ double difference = fabs( reading - this->desired_adc_value );
+ double adjustment = difference / acceleration_factor / this->readings_per_second;
+ if( reading > this->tail_adc_value ){
+ this->heater_pwm->write( 1 );
+ }else if( reading < this->head_adc_value ){
+ this->pwm_value -= adjustment;
+ this->heater_pwm->write( 0 );
+ }else{
+ if( reading > this->desired_adc_value ){
+ this->pwm_value += adjustment; // Heat up
+ }else{
+ this->pwm_value -= adjustment; // Heat down
+ }
+ this->pwm_value = max( double(0), min( double(1), pwm_value ) );
+ this->heater_pwm->write( pwm_value );
+ }
+ }
+
+}
+
+double TemperatureControl::new_thermistor_reading(){
+ double new_reading = this->thermistor_pin->read();
+
+ if( this->queue.size() < 15 ){
+ this->queue.push_back( new_reading );
+ //this->kernel->serial->printf("first\r\n");
+ return new_reading;
+ }else{
+ double current_temp = this->average_adc_reading();
+ double error = fabs(new_reading - current_temp);
+ if( error < 0.1 ){
+ this->error_count = 0;
+ double test;
+ this->queue.pop_front(test);
+ this->queue.push_back( new_reading );
+ }else{
+ this->error_count++;
+ if( this->error_count > 4 ){
+ double test;
+ this->queue.pop_front(test);
+ }
+ }
+ return current_temp;
+ }
+}
+
+
+double TemperatureControl::average_adc_reading(){
+ double total;
+ int j=0;
+ int reading_index = this->queue.head;
+ while( reading_index != this->queue.tail ){
+ j++;
+ total += this->queue.buffer[reading_index];
+ reading_index = this->queue.next_block_index( reading_index );
+ }
+ return total / j;
+}
+
+
+
--- /dev/null
+#ifndef TEMPERATURECONTROL_H
+#define TEMPERATURECONTROL_H
+
+#include "mbed.h"
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include <math.h>
+
+#define UNDEFINED -1
+
+class TemperatureControl : public Module {
+ public:
+ TemperatureControl();
+
+ void on_module_loaded();
+ void on_gcode_execute(void* argument);
+ void set_desired_temperature(double desired_temperature);
+ double get_temperature();
+ double adc_value_to_temperature(double adc_value);
+ double temperature_to_adc_value(double temperature);
+ void thermistor_read_tick();
+ double new_thermistor_reading();
+ double average_adc_reading();
+
+
+ AnalogIn* thermistor_pin;
+ PwmOut* heater_pwm;
+ double pwm_value;
+ double desired_adc_value;
+ double tail_adc_value;
+ double head_adc_value;
+
+ // Thermistor computation settings
+ double r0;
+ double t0;
+ double beta;
+ double vadc;
+ double vcc;
+ double k;
+ double vs;
+ double rs;
+
+ double acceleration_factor;
+ double readings_per_second;
+
+ RingBuffer<double,16> queue; // Queue of Blocks
+ int error_count;
+};
+
+#endif
HCoop / clinton / Smoothieware.git / commitdiff