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 = 50;
+
+ 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(){
+ 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;
+ for( int i = 0; i <= this->queue.size()-1; i++ ){
+ total += *(this->queue.get_ref(i));
+ }
+ return total / this->queue.size();
+ }
+
+ 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->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);
}
-
}
void Extruder::on_module_loaded() {
+ this->debug = false;
+
if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; }
extruder_for_irq = this;
void Extruder::on_speed_change(void* argument){
+
+ if( this->debug ){ this->kernel->serial->printf("back in event \r\n");wait(0.1); }
+
// Set direction
if( this->target_position > this->current_position ){
this->direction = 1;
if( fabs(this->travel_ratio) > 0.001 ){
+ if( this->debug ){
+ this->kernel->serial->printf("back in if \r\n");
+ this->kernel->stepper->current_block->debug(this->kernel);
+ wait(0.1);
+ }
+
// Do not change rate if stepper rate is null
if( this->kernel->stepper->current_block == NULL || fabs(this->kernel->stepper->trapezoid_adjusted_rate) < 0.0001 || this->kernel->stepper->current_block->nominal_rate == 0 ){
- return;
- }
+ return;
+ }
+
+
+ if( this->debug ){ this->kernel->serial->printf("back in if2 \r\n");wait(0.1); }
// Get a current/nominal rate ratio
+ if( fabs(this->kernel->stepper->current_block->nominal_rate) < 0.01 ){
+ wait(1);
+ }
+
+
+ if( this->debug ){ this->kernel->serial->printf("back in if3 \r\n");wait(0.1); }
+
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
+
+
+ if( this->debug ){ this->kernel->serial->printf("back in if4 \r\n");wait(0.1); }
+
double nominal_duration = this->kernel->stepper->current_block->millimeters / ( this->kernel->stepper->current_block->nominal_speed / 60 ) ;
+
+ bool testo = false;
+ if( this->debug ){
+ this->kernel->serial->printf("back in if5 \r\n");wait(0.1);
+ this->debug_count--;
+ if( this->debug_count == 0 ){
+ this->debug = false;
+ }
+ testo = true;
+ }
+ if( testo ){ this->kernel->serial->printf("back in if7 \r\n");wait(0.1); }
+
// Get extrusion nominal speed
+ if( fabs(nominal_duration) < 0.001 || this->debug ){
+ this->kernel->serial->printf("start debugging, nominal_duration: %f \r\n", double(nominal_duration));
+ this->kernel->stepper->current_block->debug(this->kernel);
+ this->debug = true;
+ this->debug_count = 5;
+ wait(0.1);
+ }
+
+ if( testo ){ this->kernel->serial->printf("back in if8 \r\n");wait(0.1); }
double nominal_extrusion_speed = fabs( this->target_position - this->start_position ) / nominal_duration;
+
+
+ if( testo ){ this->kernel->serial->printf("back in if9 \r\n");wait(0.1); }
+ if( this->debug ){ this->kernel->serial->printf("nom_extr_speed: %f \r\n", double(nominal_extrusion_speed));wait(0.1); }
// Get adjusted speed
double adjusted_speed = nominal_extrusion_speed * stepper_rate_ratio;
+
+ if( testo ){ this->kernel->serial->printf("back in if10 \r\n");wait(0.1); }
+
+ if( this->debug ){ this->kernel->serial->printf("adj_speed a: %f \r\n", double(adjusted_speed));wait(0.1); }
//this->kernel->serial->printf("e:direction: %d start:%f current: %f target: %f add:%f close:%f \r\n", this->direction, this->start_position, this->current_position, this->target_position, (double(double(1)/double(1001)))*double(this->direction), fabs( this->current_position - this->target_position ) );
//this->kernel->serial->printf("speed change rate:%f/%u=%f dur:%f=%f/%f nom_extr_spd:%f/%f=%f adj_speed:%f \r\n", this->kernel->stepper->trapezoid_adjusted_rate, this->kernel->stepper->current_block->nominal_rate, stepper_rate_ratio, nominal_duration, this->kernel->stepper->current_block->millimeters, this->kernel->stepper->current_block->nominal_speed / 60, ( this->target_position - this->start_position ), nominal_duration, nominal_extrusion_speed, adjusted_speed );
+
+ if( testo ){ this->kernel->serial->printf("back in if11 \r\n");wait(0.1); }
+ if( this->debug ){ this->kernel->serial->printf("adj_speed b: %f \r\n", double(adjusted_speed));wait(0.1); }
// Set timer
LPC_TIM1->MR0 = ((SystemCoreClock/4))/int(floor(adjusted_speed*1000));
+ if( testo ){ this->kernel->serial->printf("back in if12 \r\n");wait(0.1); }
+ if( this->debug ){ this->kernel->serial->printf("adj_speed c: %f \r\n", double(adjusted_speed));wait(0.1); }
// 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;
// Update Timer1
LPC_TIM1->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
+ if( testo ){ this->kernel->serial->printf("back in if13 \r\n");wait(0.1); }
+ if( this->debug ){ this->kernel->serial->printf("adj_speed d: %f \r\n", double(adjusted_speed));wait(1); }
}
if( fabs(this->travel_distance) > 0.001 ){
}
-
-
}
HCoop / clinton / Smoothieware.git / commitdiff