/*! \brief Move the stepper motor a given number of steps.
*
* Makes the stepper motor move the given number of steps.
* It accelrate with given accelration up to maximum speed and decelerate
* with given deceleration so it stops at the given step.
* If accel/decel is to small and steps to move is to few, speed might not
* reach the max speed limit before deceleration starts.
*
* \param step Number of steps to move (pos - CW, neg - CCW).
* \param accel Accelration to use, in 0.01*rad/sec^2.
* \param speed Max speed, in 0.01*rad/sec.
*/
void speed_cntr_Move(signed int soll_pos, unsigned int accel, unsigned long speed)
{
//! Number of steps before we hit max speed.
unsigned int step_to_max_speed;
//! Number of steps before we must start deceleration (if accel does not hit max speed).
unsigned int accel_lim;
unsigned int differ_steps;
// Set direction from differ of step value.
if(soll_pos < srd.position){
srd.dir = CCW;
differ_steps=srd.position-soll_pos;
} else {
srd.dir = CW;
differ_steps=soll_pos-srd.position;
}
// If moving only 1 step.
if(differ_steps == 1){
// Move one step...
srd.accel_steps = -1;
// ...in DECEL state.
srd.run_state = DECEL;
// Just a short delay so main() can act on 'running'.
srd.step_delay = 1000;
status.running = TRUE;
OCR1A = 10;
// Run Timer/Counter 1 with prescaler = 1.
TCCR1B |= (1<<CS10);
}
// Only move if number of steps to move is not zero.
else if(differ_steps != 0){
// Refer to documentation for detailed information about these calculations.
if(srd.run_state==STOP){
// Set max speed limit, by calc min_delay to use in timer.
// min_delay = (alpha / tt)/ w
srd.min_delay = A_T_x100 / speed;
// Set accelration by calc the first (c0) step delay .
// step_delay = 1/tt * sqrt(2*alpha/accel)
// step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000
srd.step_delay = (T1_FREQ_148 * sqrt_2(A_SQ / accel))/100;
// Find out after how many steps does the speed hit the max speed limit.
// step_to_max_speed = speed^2 / (2*alpha*accel)
step_to_max_speed = (long)speed*speed/(long)(((long)A_x20000*accel)/100);
// If we hit max speed limit before 0,5 step it will round to 0.
// But in practice we need to move atleast 1 step to get any speed at all.
if(step_to_max_speed == 0){
step_to_max_speed = 1;
}
// wenn wir für beschleunigung und entschleunigung zusammen MEHR als die gesamt schritte bräuchten
if((step_to_max_speed<<1) >= differ_steps){
srd.decel_steps_neg = -(differ_steps>>1); // negativ soviel schritte, wie ich positiv zum speedup gebraucht hab
if(srd.dir==CCW){
srd.decel_start = srd.position - (differ_steps>>1);
} else { // der motor dreht hoch, dann ist
int sqrt_2(int n, int left, int right){
int left2 = left * left;
int right2 = right * right;
if(left2 == n) return left;
if(right2 == n) return right;
int middle = (left + right) / 2;
int middle2 = middle * middle;
if(middle2 == n) return middle;
if(left2 < n && n < middle2){
return sqrt_2(n, left + 1, middle - 1);
}else if(middle2 < n && n < right2){
return sqrt_2(n, middle + 1, right - 1);
}else if(n < left2){
return left - 1;
}else if(n > right2){
return right;
}
}
int sqrt2(int n){
return sqrt_2(n, 0, n);
}
