コード例 #1
0
ファイル: heaters.c プロジェクト: CNCBASHER/4pi-firmware
//--------------------------------------------------
// Simple Hotend Tempcontrol with ON/OFF switching
//--------------------------------------------------
void heater_on_off_control(heater_struct *hotend)
{
	hotend->akt_temp = analog2temp(adc_read(hotend->ad_cannel));
	
	if(hotend->akt_temp < 4)
	{
		hotend->target_temp = 0;
		heater_switch(hotend->io_adr, 0);
	}
		
	if(hotend->akt_temp  > (hotend->target_temp+1))
	{
		heater_switch(hotend->io_adr, 0);
		hotend->pwm = 0;
	}
	else if((hotend->akt_temp  < (hotend->target_temp-1)) && (hotend->target_temp > 0))
	{
		heater_switch(hotend->io_adr, 1);
		hotend->pwm = 255;
	
	}
	
}
コード例 #2
0
ファイル: temperature.cpp プロジェクト: Botronicz/Marlin
void PID_autotune(float temp)
{
  float input;
  int cycles=0;
  bool heating = true;

  unsigned long temp_millis = millis();
  unsigned long t1=temp_millis;
  unsigned long t2=temp_millis;
  long t_high;
  long t_low;

  long bias=127;
  long d = 127;
  float Ku, Tu;
  float Kp, Ki, Kd;
  float max, min;
  
  SERIAL_ECHOLN("PID Autotune start");
  
  disable_heater(); // switch off all heaters.
  
  soft_pwm[0] = 255>>1;
    
  for(;;) {

    if(temp_meas_ready == true) { // temp sample ready
      CRITICAL_SECTION_START;
      temp_meas_ready = false;
      CRITICAL_SECTION_END;
      input = analog2temp(current_raw[0], 0);
      
      max=max(max,input);
      min=min(min,input);
      if(heating == true && input > temp) {
        if(millis() - t2 > 5000) { 
          heating=false;
          soft_pwm[0] = (bias - d) >> 1;
          t1=millis();
          t_high=t1 - t2;
          max=temp;
        }
      }
      if(heating == false && input < temp) {
        if(millis() - t1 > 5000) {
          heating=true;
          t2=millis();
          t_low=t2 - t1;
          if(cycles > 0) {
            bias += (d*(t_high - t_low))/(t_low + t_high);
            bias = constrain(bias, 20 ,235);
            if(bias > 127) d = 254 - bias;
            else d = bias;

            SERIAL_PROTOCOLPGM(" bias: "); SERIAL_PROTOCOL(bias);
            SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL(d);
            SERIAL_PROTOCOLPGM(" min: "); SERIAL_PROTOCOL(min);
            SERIAL_PROTOCOLPGM(" max: "); SERIAL_PROTOCOLLN(max);
            if(cycles > 2) {
              Ku = (4.0*d)/(3.14159*(max-min)/2.0);
              Tu = ((float)(t_low + t_high)/1000.0);
              SERIAL_PROTOCOLPGM(" Ku: "); SERIAL_PROTOCOL(Ku);
              SERIAL_PROTOCOLPGM(" Tu: "); SERIAL_PROTOCOLLN(Tu);
              Kp = 0.6*Ku;
              Ki = 2*Kp/Tu;
              Kd = Kp*Tu/8;
              SERIAL_PROTOCOLLNPGM(" Clasic PID ")
              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
              /*
              Kp = 0.33*Ku;
              Ki = Kp/Tu;
              Kd = Kp*Tu/3;
              SERIAL_PROTOCOLLNPGM(" Some overshoot ")
              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
              Kp = 0.2*Ku;
              Ki = 2*Kp/Tu;
              Kd = Kp*Tu/3;
              SERIAL_PROTOCOLLNPGM(" No overshoot ")
              SERIAL_PROTOCOLPGM(" Kp: "); SERIAL_PROTOCOLLN(Kp);
              SERIAL_PROTOCOLPGM(" Ki: "); SERIAL_PROTOCOLLN(Ki);
              SERIAL_PROTOCOLPGM(" Kd: "); SERIAL_PROTOCOLLN(Kd);
              */
            }
          }
          soft_pwm[0] = (bias + d) >> 1;
          cycles++;
          min=temp;
        }
      } 
コード例 #3
0
ファイル: temperature.cpp プロジェクト: BadgerOps/reprap
void PID_autotune(float temp, int extruder, int ncycles)
{
  float input = 0.0;
  int cycles=0;
  bool heating = true;

  unsigned long temp_millis = millis();
  unsigned long t1=temp_millis;
  unsigned long t2=temp_millis;
  long t_high = 0;
  long t_low = 0;

  long bias, d;
  float Ku, Tu;
  float Kp, Ki, Kd;
  float max = 0, min = 10000;

	if ((extruder > EXTRUDERS)
  #if (TEMP_BED_PIN <= -1)
		||(extruder < 0)
	#endif
	){
  	SERIAL_ECHOLN("PID Autotune failed. Bad extruder number.");
  	return;
	}
	
  SERIAL_ECHOLN("PID Autotune start");
  
  disable_heater(); // switch off all heaters.

	if (extruder<0)
	{
	 	soft_pwm_bed = (MAX_BED_POWER)/2;
		bias = d = (MAX_BED_POWER)/2;
  }
	else
	{
	  soft_pwm[extruder] = (PID_MAX)/2;
		bias = d = (PID_MAX)/2;
  }




 for(;;) {

    if(temp_meas_ready == true) { // temp sample ready
      CRITICAL_SECTION_START;
      temp_meas_ready = false;
      CRITICAL_SECTION_END;
      input = (extruder<0)?analog2tempBed(current_raw_bed):analog2temp(current_raw[extruder], extruder);

      max=max(max,input);
      min=min(min,input);
      if(heating == true && input > temp) {
        if(millis() - t2 > 5000) { 
          heating=false;
					if (extruder<0)
						soft_pwm_bed = (bias - d) >> 1;
					else
						soft_pwm[extruder] = (bias - d) >> 1;
          t1=millis();
          t_high=t1 - t2;
          max=temp;
        }
      }
コード例 #4
0
ファイル: heaters.c プロジェクト: CNCBASHER/4pi-firmware
//--------------------------------------------------
// Tempcontrol with PID for Hotend
//--------------------------------------------------
void heater_PID_control(heater_struct *hotend)
{
	signed short error;
	signed short delta_temp;
	signed short heater_duty;
  
	hotend->akt_temp = analog2temp(adc_read(hotend->ad_cannel));
  
	//MIN save to display the jitter of Heaterbarrel
	#ifdef MINTEMP
	if(hotend->akt_temp < 4)
	{
		hotend->target_temp = 0;
		heater_switch(hotend->io_adr, 0);
	}
	#endif
	
	//MAX save to display the jitter of Heaterbarrel
	#ifdef MAXTEMP
	if(hotend->akt_temp > 300)
	{
		hotend->target_temp = 0;
		heater_switch(hotend->io_adr, 0);
	}
	#endif

	error = hotend->target_temp - hotend->akt_temp;
	//printf("ERR: %d ", error);
	delta_temp = hotend->akt_temp - hotend->prev_temp;

	hotend->prev_temp = hotend->akt_temp;
	hotend->pTerm = (signed short)(((long)hotend->PID_Kp * error) / 256);
	
	const signed short H0 = min(HEATER_DUTY_FOR_SETPOINT(hotend->target_temp),HEATER_CURRENT);
	heater_duty = H0 + hotend->pTerm;

	if(abs(error) < 30)
	{
		hotend->temp_iState += error;
		//printf("I1: %d ", hotend->temp_iState);
		hotend->temp_iState = constrain(hotend->temp_iState, hotend->temp_iState_min, hotend->temp_iState_max);
		//printf("I2: %d ", hotend->temp_iState);
		hotend->iTerm = (signed short)(((long)hotend->PID_I * hotend->temp_iState) / 256);
		heater_duty += hotend->iTerm;
	}
	
	//printf("I: %d ", hotend->iTerm);

	signed short prev_error = abs(hotend->target_temp - hotend->prev_temp);
	signed short log3 = 1; // discrete logarithm base 3, plus 1

	if(prev_error > 81){ prev_error /= 81; log3 += 4; }
	if(prev_error >  9){ prev_error /=  9; log3 += 2; }
	if(prev_error >  3){ prev_error /=  3; log3 ++;   }

	hotend->dTerm = (signed short)(((long)hotend->PID_Kd * delta_temp) / (256*log3));
	
	//printf("D: %d ", hotend->dTerm);
	
	heater_duty += hotend->dTerm;
	//printf("PWM: %d \n", heater_duty);
	
	heater_duty = constrain(heater_duty, 0, HEATER_CURRENT);
	


	if(hotend->target_temp == 0)
		hotend->pwm = 0;
	else
		hotend->pwm = (unsigned char)heater_duty;


}
コード例 #5
0
void process_commands() {
    unsigned long codenum; //throw away variable

    if (code_seen('N')) {
        gcode_N = code_value_long();
        if (gcode_N != gcode_LastN+1 && (strstr(cmdbuffer, "M110") == NULL) ) {
            gcode_LastN=0;
            pc.printf("ok");
            //if(gcode_N != gcode_LastN+1 && !code_seen("M110") ) {   //Hmm, compile size is different between using this vs the line above even though it should be the same thing. Keeping old method.
            //pc.printf("Serial Error: Line Number is not Last Line Number+1, Last Line:");
            //pc.printf("%d\n",gcode_LastN);
            //FlushSerialRequestResend();
            return;
        }

        if (code_seen('*')) {
            int checksum = 0;
            int count=0;
            while (cmdbuffer[count] != '*') checksum = checksum^cmdbuffer[count++];

            if ( (int)code_value() != checksum) {
                //pc.printf("Error: checksum mismatch, Last Line:");
                //pc.printf("%d\n",gcode_LastN);
                //FlushSerialRequestResend();
                return;
            }
            //if no errors, continue parsing
        } else {
            //pc.printf("Error: No Checksum with line number, Last Line:");
            //pc.printf("%d\n",gcode_LastN);
            //FlushSerialRequestResend();
            return;
        }

        gcode_LastN = gcode_N;
        //if no errors, continue parsing
    } else { // if we don't receive 'N' but still see '*'
        if (code_seen('*')) {
            //pc.printf("Error: No Line Number with checksum, Last Line:");
            //pc.printf("%d\n",gcode_LastN);
            return;
        }
    }

    //continues parsing only if we don't receive any 'N' or '*' or no errors if we do. :)

    if (code_seen('G')) {
        switch ((int)code_value()) {
            case 0: // G0 -> G1
            case 1: // G1
                reset_timers();//avoid timer overflow after 30 seconds
                get_coordinates(); // For X Y Z E F
                x_steps_to_take = abs(destination_x - current_x)*x_steps_per_unit;
                y_steps_to_take = abs(destination_y - current_y)*y_steps_per_unit;
                z_steps_to_take = abs(destination_z - current_z)*z_steps_per_unit;
                e_steps_to_take = abs(destination_e - current_e)*e_steps_per_unit;
                //printf(" x_steps_to_take:%d\n", x_steps_to_take);


                time_for_move = max(X_TIME_FOR_MOVE,Y_TIME_FOR_MOVE);
                time_for_move = max(time_for_move,Z_TIME_FOR_MOVE);
                time_for_move = max(time_for_move,E_TIME_FOR_MOVE);

                if (x_steps_to_take) x_interval = time_for_move/x_steps_to_take;
                if (y_steps_to_take) y_interval = time_for_move/y_steps_to_take;
                if (z_steps_to_take) z_interval = time_for_move/z_steps_to_take;
                if (e_steps_to_take) e_interval = time_for_move/e_steps_to_take;


                x_steps_remaining = x_steps_to_take;
                y_steps_remaining = y_steps_to_take;
                z_steps_remaining = z_steps_to_take;
                e_steps_remaining = e_steps_to_take;


                if (DEBUGGING) {
                    pc.printf("destination_x: %f\n",destination_x);
                    pc.printf("current_x: %f\n",current_x);
                    pc.printf("x_steps_to_take: %d\n",x_steps_to_take);
                    pc.printf("X_TIME_FOR_MOVE: %f\n",X_TIME_FOR_MOVE);
                    pc.printf("x_interval: %f\n\n",x_interval);

                    pc.printf("destination_y: %f\n",destination_y);
                    pc.printf("current_y: %f\n",current_y);
                    pc.printf("y_steps_to_take: %d\n",y_steps_to_take);
                    pc.printf("Y_TIME_FOR_MOVE: %f\n",Y_TIME_FOR_MOVE);
                    pc.printf("y_interval: %f\n\n",y_interval);

                    pc.printf("destination_z: %f\n",destination_z);
                    pc.printf("current_z: %f\n",current_z);
                    pc.printf("z_steps_to_take: %d\n",z_steps_to_take);
                    pc.printf("Z_TIME_FOR_MOVE: %f\n",Z_TIME_FOR_MOVE);
                    pc.printf("z_interval: %f\n\n",z_interval);

                    pc.printf("destination_e: %f\n",destination_e);
                    pc.printf("current_e: %f\n",current_e);
                    pc.printf("e_steps_to_take: %d\n",e_steps_to_take);
                    pc.printf("E_TIME_FOR_MOVE: %f\n",E_TIME_FOR_MOVE);
                    pc.printf("e_interval: %f\n\n",e_interval);
                }

                linear_move(); // make the move
                ClearToSend();
                return;
            case 4: // G4 dwell
                codenum = 0;
                if (code_seen('P')) codenum = code_value(); // milliseconds to wait
                if (code_seen('S')) codenum = code_value()*1000; // seconds to wait
                previous_millis_heater = millis();  // keep track of when we started waiting
                while ((millis() - previous_millis_heater) < codenum ) manage_heater(); //manage heater until time is up
                break;
            case 90: // G90
                relative_mode = false;
                break;
            case 91: // G91
                relative_mode = true;
                break;
            case 92: // G92
                if (code_seen('X')) current_x = code_value();
                if (code_seen('Y')) current_y = code_value();
                if (code_seen('Z')) current_z = code_value();
                if (code_seen('E')) current_e = code_value();
                break;
           case 93: // G93
                pc.printf("previous_micros:%d\n", previous_micros);
                pc.printf("previous_micros_x:%d\n", previous_micros_x);
                pc.printf("previous_micros_y:%d\n", previous_micros_y);
                pc.printf("previous_micros_z:%d\n", previous_micros_z);
                break;

        }
    }

    if (code_seen('M')) {

        switch ( (int)code_value() ) {
            case 104: // M104 - set hot-end temp
                
                if (code_seen('S'))
                {
                     
                    target_raw = temp2analog(code_value());
                    //pc.printf("target_raw: %d\n ", target_raw);
                }
                break;
        case 140: // M140 - set heated-printbed temp
                if (code_seen('S'))
                {
                     
                    target_raw1 = temp2analog(code_value());
                    //pc.printf("target_raw1: %d\n ", target_raw);
                }
                break;                
                
            case 105: // M105
                pc.printf("ok T:");
                if (TEMP_0_PIN != NC) {
                    pc.printf("%f\n", analog2temp( (p_temp0.read_u16())  ));
                } else {
                    pc.printf("0.0\n");
                }
                if (!code_seen('N')) return; // If M105 is sent from generated gcode, then it needs a response.
                break;
            case 109: // M109 - Wait for heater to reach target.
                if (code_seen('S')) target_raw = temp2analog(code_value());
                previous_millis_heater = millis();
                while (current_raw < target_raw) {
                    if ( (millis()-previous_millis_heater) > 1000 ) { //Print Temp Reading every 1 second while heating up.
                        pc.printf("ok T:");
                        if (TEMP_0_PIN != NC) {
                            pc.printf("%f\n", analog2temp(p_temp0.read_u16()));
                        } else {
                            pc.printf("0.0\n");
                        }
                        previous_millis_heater = millis();
                    }
                    manage_heater();
                }
                break;
            case 106: //M106 Fan On
                p_fan = 1;
                break;
            case 107: //M107 Fan Off
                p_fan = 0;
                break;
            case 80: // M81 - ATX Power On
                //if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,OUTPUT); //GND
                break;
            case 81: // M81 - ATX Power Off
                //if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT); //Floating
                break;
            case 82:
                relative_mode_e = false;
                break;
            case 83:
                relative_mode_e = true;
                break;
            case 84:
                disable_x();
                disable_y();
                disable_z();
                disable_e();
                break;
            case 85: // M85
                code_seen('S');
                max_inactive_time = code_value()*1000;
                break;
            case 86: // M86 If Endstop is Not Activated then Abort Print
                if (code_seen('X')) {
                    if (X_MIN_PIN != NC) {
                        if ( p_X_min == ENDSTOPS_INVERTING ) {
                            kill(3);
                        }
                    }
                }
                if (code_seen('Y')) {
                    if (Y_MIN_PIN != NC) {
                        if ( p_Y_min == ENDSTOPS_INVERTING ) {
                            kill(4);
                        }
                    }
                }
                break;
            case 92: // M92
                if (code_seen('X')) x_steps_per_unit = code_value();
                if (code_seen('Y')) y_steps_per_unit = code_value();
                if (code_seen('Z')) z_steps_per_unit = code_value();
                if (code_seen('E')) e_steps_per_unit = code_value();
                break;
        }

    }

    ClearToSend();
}
コード例 #6
0
ファイル: temperature.cpp プロジェクト: skapin/RepRapFirmware
void manage_heater()
{
  #ifdef USE_WATCHDOG
    wd_reset();
  #endif
  
  float pid_input;
  float pid_output;

  if(temp_meas_ready != true)   //better readability
    return; 

  CRITICAL_SECTION_START;
  temp_meas_ready = false;
  CRITICAL_SECTION_END;

  for(int e = 0; e < EXTRUDERS; e++) 
  {

  #ifdef PIDTEMP
    pid_input = analog2temp(current_raw[e], e);

    #ifndef PID_OPENLOOP
        pid_error[e] = pid_setpoint[e] - pid_input;
        if(pid_error[e] > 10) {
          pid_output = PID_MAX;
          pid_reset[e] = true;
        }
        else if(pid_error[e] < -10) {
          pid_output = 0;
          pid_reset[e] = true;
        }
        else {
          if(pid_reset[e] == true) {
            temp_iState[e] = 0.0;
            pid_reset[e] = false;
          }
          pTerm[e] = Kp * pid_error[e];
          temp_iState[e] += pid_error[e];
          temp_iState[e] = constrain(temp_iState[e], temp_iState_min[e], temp_iState_max[e]);
          iTerm[e] = Ki * temp_iState[e];
          //K1 defined in Configuration.h in the PID settings
          #define K2 (1.0-K1)
          dTerm[e] = (Kd * (pid_input - temp_dState[e]))*K2 + (K1 * dTerm[e]);
          temp_dState[e] = pid_input;
          pid_output = constrain(pTerm[e] + iTerm[e] - dTerm[e], 0, PID_MAX);
        }
    #endif //PID_OPENLOOP
    #ifdef PID_DEBUG
    SERIAL_ECHOLN(" PIDDEBUG "<<e<<": Input "<<pid_input<<" Output "<<pid_output" pTerm "<<pTerm[e]<<" iTerm "<<iTerm[e]<<" dTerm "<<dTerm[e]);  
    #endif //PID_DEBUG
  #else /* PID off */
    pid_output = 0;
    if(current_raw[e] < target_raw[e]) {
      pid_output = PID_MAX;
    }
  #endif

    // Check if temperature is within the correct range
    if((current_raw[e] > minttemp[e]) && (current_raw[e] < maxttemp[e])) 
    {
      //analogWrite(heater_pin_map[e], pid_output);
      soft_pwm[e] = (int)pid_output >> 1;
    }
    else {
コード例 #7
0
ファイル: planner.c プロジェクト: glocklueng/5DPrint-Firmware
// 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) {
    int current_temp;
  
    // Calculate the buffer head after we push this byte
    int next_buffer_head = next_block_index(block_buffer_head);

    // 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_inactivity(1); 
#if (MINIMUM_FAN_START_SPEED > 0)
        manage_fan_start_speed();
#endif 
    }

    // The target position of the tool in absolute steps
    // Calculate target position in absolute steps
    //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
    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]);
  
    current_temp = analog2temp( current_raw );
  
#if PREVENT_DANGEROUS_EXTRUDE > 0
    if(target[E_AXIS]!=position[E_AXIS]) {
        if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) {
            position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
            serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF);
        }
    
#if PREVENT_LENGTHY_EXTRUDE > 0
        if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
            position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
            serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF);
        }
#endif
    }
#endif
    
    // 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 = 0;

    // 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->steps_e = (long)(block->steps_e * extrudemultiply / 100.0);
    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 <= DROP_SEGMENTS) return;

    // 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); 
            //High Feedrate for retract
            max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
            retract_feedrate_aktiv = 1;
    }
    else {
        if(retract_feedrate_aktiv) {
                if(block->steps_e > 0) retract_feedrate_aktiv = 0;
        }
        else max_E_feedrate_calc = max_feedrate[E_AXIS]; 
    }

#ifdef DELAY_ENABLE
    if(block->steps_x != 0) {
        enable_x();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_y != 0) {
        enable_y();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_z != 0) {
        enable_z();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_e != 0) {
        enable_e();
        delayMicroseconds(DELAY_ENABLE);
    }
#else
    //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();
#endif 
 
    if (block->steps_e == 0) {
        if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
    }
    else {
    	if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
    } 

    // slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
    int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask;
#ifdef SLOWDOWN  
    if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN)
        feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5); 
#endif

    float delta_mm[4];
    delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]);
    delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]);
    delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]);
    //delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
    delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0;
  
    if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS )
        block->millimeters = fabs(delta_mm[E_AXIS]);
    else
        block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
  
    float inverse_millimeters = 1.0/(float)(block->millimeters);  // Inverse millimeters to remove multiple divides 
  
    // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
    float inverse_second = feed_rate * inverse_millimeters;
  
    block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
    block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
  
    /*
      #ifdef SLOWDOWN
      //  segment time im micro seconds
      long segment_time = lround(1000000.0/inverse_second);
      if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) {
      if (segment_time<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
      segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued);
      }
      }
      else {
      if (segment_time<minsegmenttime) segment_time=minsegmenttime;
      }
      #endif
      //  END OF SLOW DOWN SECTION
      */

    // Calculate and limit speed in mm/sec for each axis
    float current_speed[4];
    float speed_factor = 1.0; //factor <=1 do decrease speed
    for(int i=0; i < 3; i++) {
        current_speed[i] = delta_mm[i] * inverse_second;
        if(fabs(current_speed[i]) > max_feedrate[i])
            speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
    }
  
    current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
    if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
        speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));


    // Correct the speed  
    if( speed_factor < 1.0)  {
            for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor;
            block->nominal_speed *= speed_factor;
            block->nominal_rate *= speed_factor;
    }
    
    // Compute and limit the acceleration rate for the trapezoid generator.  
    float steps_per_mm = block->step_event_count/(float)(block->millimeters);
    if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) 
        block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
    else {
        block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
        // Limit acceleration per axis
        if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
	
            block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
    }
  
    block->acceleration = block->acceleration_st / (float)(steps_per_mm);
    block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
  
#if 0  // Use old jerk for now
    // Compute path unit vector
    double unit_vec[3];

    unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
    unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
    unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
  
    // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
    // Let a circle be tangent to both previous and current path line segments, where the junction
    // deviation is defined as the distance from the junction to the closest edge of the circle,
    // colinear with the circle center. The circular segment joining the two paths represents the
    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
    // path width or max_jerk in the previous grbl version. This approach does not actually deviate
    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
    // nonlinearities of both the junction angle and junction velocity.
    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed

    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
        // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
        // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
        double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
            - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
            - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
                           
        // Skip and use default max junction speed for 0 degree acute junction.
        if (cos_theta < 0.95) {
            vmax_junction = min(previous_nominal_speed,block->nominal_speed);
            // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
            if (cos_theta > -0.95) {
                // Compute maximum junction velocity based on maximum acceleration and junction deviation
                double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
                vmax_junction = min(vmax_junction,
                                    sqrt(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) );
            }
        }
    }
#endif
    // Start with a safe speed
    float vmax_junction = max_xy_jerk/2.0; 
    float vmax_junction_factor = 1.0; 

    if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
    if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
    if(G92_reset_previous_speed == 1) {
            vmax_junction = 0.1;
            G92_reset_previous_speed = 0;  
    }

    vmax_junction = fmin(vmax_junction, block->nominal_speed);
    float safe_speed = vmax_junction;

    if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
        float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
        //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
        vmax_junction = block->nominal_speed;
        //    }
        if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
        if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
            vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
        if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
            vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
        vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }
  
  
    block->max_entry_speed = vmax_junction;

    // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
    double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
    block->entry_speed = fmin(vmax_junction, v_allowable);
  
    // Initialize planner efficiency flags
    // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
    // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
    // the current block and next block junction speeds are guaranteed to always be at their maximum
    // junction speeds in deceleration and acceleration, respectively. This is due to how the current
    // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
    // the reverse and forward planners, the corresponding block junction speed will always be at the
    // the maximum junction speed and may always be ignored for any speed reduction checks.
    if (block->nominal_speed <= v_allowable) block->nominal_length_flag = 1;
    else block->nominal_length_flag = 0;
    block->recalculate_flag = 1; // Always calculate trapezoid for new block
  
    // Update previous path unit_vector and nominal speed
    memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
    previous_nominal_speed = block->nominal_speed;
  
    calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
                                  safe_speed/(float)(block->nominal_speed));
    
    // Move buffer head
    CRITICAL_SECTION_START;
    block_buffer_head = next_buffer_head;
    CRITICAL_SECTION_END;
  
    // Update position
    memcpy(position, target, sizeof(target)); // position[] = target[]

    planner_recalculate();
#ifdef AUTOTEMP
    getHighESpeed();
#endif
    st_wake_up();
}
コード例 #8
0
ファイル: heater.cpp プロジェクト: ohmbase/mix-g1-3dprinter
 void manage_heater()
 {

  //Temperatur Monitor for repetier
  if((millis() - previous_millis_monitor) > 250 )
  {
    previous_millis_monitor = millis();
    if(manage_monitor <= 1)
    {
      showString(PSTR("MTEMP:"));
      Serial.print(millis());
      if(manage_monitor<1)
      {
        showString(PSTR(" "));
        Serial.print(analog2temp(current_raw));
        showString(PSTR(" "));
        Serial.print(target_temp);
        showString(PSTR(" "));
        #ifdef PIDTEMP
        Serial.println(heater_duty);
        #else 
          #if (HEATER_0_PIN > -1)
          if(READ(HEATER_0_PIN))
            Serial.println(255);
          else
            Serial.println(0);
          #else
          Serial.println(0);
          #endif
        #endif
      }
      #if THERMISTORBED!=0
      else
      {
        showString(PSTR(" "));
        Serial.print(analog2tempBed(current_bed_raw));
        showString(PSTR(" "));
        Serial.print(analog2tempBed(target_bed_raw));
        showString(PSTR(" "));
        #if (HEATER_1_PIN > -1)
          if(READ(HEATER_1_PIN))
            Serial.println(255);
          else
            Serial.println(0);
        #else
          Serial.println(0);
        #endif  
      }
      #endif
      
    }
  
  }
  // ENDE Temperatur Monitor for repetier
 
  if((millis() - previous_millis_heater) < HEATER_CHECK_INTERVAL )
    return;
    
  previous_millis_heater = millis();
  
  #ifdef HEATER_USES_THERMISTOR
    current_raw = analogRead(TEMP_0_PIN); 
    #ifdef DEBUG_HEAT_MGMT
      log_int("_HEAT_MGMT - analogRead(TEMP_0_PIN)", current_raw);
      log_int("_HEAT_MGMT - NUMTEMPS", NUMTEMPS);
    #endif
    // When using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, 
    // this switches it up so that the reading appears lower than target for the control logic.
    current_raw = 1023 - current_raw;
  #elif defined HEATER_USES_AD595
    current_raw = analogRead(TEMP_0_PIN);    
  #elif defined HEATER_USES_MAX6675
    current_raw = read_max6675();
  #endif
  
  //MIN / MAX save to display the jitter of Heaterbarrel
  if(current_raw > current_raw_maxval)
    current_raw_maxval = current_raw;
    
  if(current_raw < current_raw_minval)
    current_raw_minval = current_raw;
 
  #ifdef SMOOTHING
    if (!nma) nma = SMOOTHFACTOR * current_raw;
    nma = (nma + current_raw) - (nma / SMOOTHFACTOR);
    current_raw = nma / SMOOTHFACTOR;
  #endif
  
  #ifdef WATCHPERIOD
    if(watchmillis && millis() - watchmillis > WATCHPERIOD)
    {
        if(watch_raw + 1 >= current_raw)
        {
            target_temp = target_raw = 0;
            WRITE(HEATER_0_PIN,LOW);

            #ifdef PID_SOFT_PWM
              g_heater_pwm_val = 0;           
            #else
              analogWrite(HEATER_0_PIN, 0);
              #if LED_PIN>-1
                WRITE(LED_PIN,LOW);
              #endif
            #endif
        }
        else
        {
            watchmillis = 0;
        }
    }
  #endif
  
  //If tmp is lower then MINTEMP stop the Heater
  //or it os better to deaktivate the uutput PIN or PWM ?
  #ifdef MINTEMP
    if(current_raw <= minttemp)
        target_temp = target_raw = 0;
  #endif
  
  #ifdef MAXTEMP
    if(current_raw >= maxttemp)
    {
        target_temp = target_raw = 0;
    
        #if (ALARM_PIN > -1) 
          WRITE(ALARM_PIN,HIGH);
        #endif
    }
  #endif

  #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675) || defined (HEATER_USES_AD595)
    #ifdef PIDTEMP
      
      int current_temp = analog2temp(current_raw);
      error = target_temp - current_temp;
      int delta_temp = current_temp - prev_temp;
      
      prev_temp = current_temp;
      pTerm = ((long)PID_PGAIN * error) / 256;
      const int H0 = min(HEATER_DUTY_FOR_SETPOINT(target_temp),HEATER_CURRENT);
      heater_duty = H0 + pTerm;
      
      if(error < 30)
      {
        temp_iState += error;
        temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max);
        iTerm = ((long)PID_IGAIN * temp_iState) / 256;
        heater_duty += iTerm;
      }
      
      int prev_error = abs(target_temp - prev_temp);
      int log3 = 1; // discrete logarithm base 3, plus 1
      
      if(prev_error > 81){ prev_error /= 81; log3 += 4; }
      if(prev_error >  9){ prev_error /=  9; log3 += 2; }
      if(prev_error >  3){ prev_error /=  3; log3 ++;   }
      
      dTerm = ((long)PID_DGAIN * delta_temp) / (256*log3);
      heater_duty += dTerm;
      heater_duty = constrain(heater_duty, 0, HEATER_CURRENT);

      #ifdef PID_SOFT_PWM
        g_heater_pwm_val = (unsigned char)heater_duty;
      #else
        analogWrite(HEATER_0_PIN, heater_duty);
    
        #if LED_PIN>-1
          analogWrite(LED_PIN, constrain(LED_PWM_FOR_BRIGHTNESS(heater_duty),0,255));
        #endif
      #endif
  
    #else
    
      if(current_raw >= target_raw)
      {
        WRITE(HEATER_0_PIN,LOW);
        #if LED_PIN>-1
            WRITE(LED_PIN,LOW);
        #endif
      }
      else 
      {
        WRITE(HEATER_0_PIN,HIGH);
        #if LED_PIN > -1
            WRITE(LED_PIN,HIGH);
        #endif
      }
    #endif
  #endif
    
  if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL)
    return;
  
  previous_millis_bed_heater = millis();

  #ifndef TEMP_1_PIN
    return;
  #endif

  #if TEMP_1_PIN == -1
    return;
  #else
  
  #ifdef BED_USES_THERMISTOR
  
    current_bed_raw = analogRead(TEMP_1_PIN);   
  
    #ifdef DEBUG_HEAT_MGMT
      log_int("_HEAT_MGMT - analogRead(TEMP_1_PIN)", current_bed_raw);
      log_int("_HEAT_MGMT - BNUMTEMPS", BNUMTEMPS);
    #endif               
  
    // If using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, 
    // this switches it up so that the reading appears lower than target for the control logic.
    current_bed_raw = 1023 - current_bed_raw;
  #elif defined BED_USES_AD595
    current_bed_raw = analogRead(TEMP_1_PIN);                  

  #endif
  
  
  #ifdef MINTEMP
    if(current_bed_raw >= target_bed_raw || current_bed_raw < minttemp)
  #else
    if(current_bed_raw >= target_bed_raw)
  #endif
    {
      WRITE(HEATER_1_PIN,LOW);
    }
    else 
    {
      WRITE(HEATER_1_PIN,HIGH);
    }
    #endif
    
#ifdef CONTROLLERFAN_PIN
  controllerFan(); //Check if fan should be turned on to cool stepper drivers down
#endif

}