diff --git a/spule.c b/spule.c index 45bdb28..9341df8 100644 --- a/spule.c +++ b/spule.c @@ -87,17 +87,28 @@ void do_spule(){ } else{ /* speed regulation - keep taenzer at 10% */ - int32_t tmp = 0;//(100 - (int32_t)taenzer_state.pos/1000); + float p = 100.0/(int32_t)(taenzer_state.pos/1000); + p-=1; + p/=2; + p+=1; //printf("temp1: %d\n", tmp); //TODO fix bounds //if(tmp < -7500/abzug_speed/2) // tmp = -7500/abzug_speed/2; //printf("temp2: %d\n", tmp); - if(tmp < -35) - tmp = -35; + if(p < 0.75) + p = 0.75; + if(p > 1.5) + p = 1.5; - ICR5=7500/abzug_speed + tmp; + uint16_t base_speed = (7500/abzug_speed); + uint16_t ctrl_speed = base_speed * p; + + if(ctrl_speed <= 70) + ctrl_speed = 70; + + ICR5 = ctrl_speed; OCR5C = ICR5/2; ICR1 = ICR5/TRANS_ROT_FACTOR;