/** Calibration routine. Must be called for a correct operation of counter registers */ void stepper_cal() { //Rotate pan CCW till index is found indexSet = 0; stepper_set_direction(0); while( !indexSet ) { _delay_ms(15); // 20 ms interstep is really slow, but securs no extra step after index is passed stepper_advance(); // comand single step advance } stepper_set_direction(1); // change direciton of rotation and some steps (this is to avoid having to manually reposition again when recalibrating to avoid) for(int8_t i=0; i<4; i++) { _delay_ms(15); // 20 ms interstep is really slow, but securs no extra step after index is passed stepper_advance(); } _delay_ms(100); // delay to let motor settle indexSet = 0; // reset index flag curr_step_num = 0; // reset step count encoder_reset(); // reset encoder counter uart_puts("L"); // from now on, the platform is calibrated. Notify host: }
void stepper_drive_speedramp(void) { static uint32_t rest = 0; uint16_t goal = stepper_velocity_goal; if((stepper_speedramp_state == STEPPER_SPEEDRAMP_STATE_STOP) || (stepper_speedramp_state != stepper_direction)) { goal = 0; } if(goal == stepper_velocity) { if(stepper_speedramp_state == STEPPER_SPEEDRAMP_STATE_STOP) { stepper_running = false; tc_channel_stop(&STEPPER_TC_CHANNEL); stepper_state = STEPPER_STATE_STOP; stepper_velocity = 0; stepper_step_counter = 0; stepper_acceleration_counter = 0; stepper_delay_rest = 0; rest = 0; return; } else if(stepper_speedramp_state != stepper_direction) { goal = stepper_velocity_goal; rest = 0; } else { // If i am at stepper velocity goal and the direction is correct // -> We have nothing to do return; } } uint16_t acceleration; uint16_t acceleration_sqrt; uint16_t deceleration; int32_t delta; if(stepper_acceleration == 0) { acceleration = 0xFFFF; acceleration_sqrt = 256; // sqrt(0xFFFF) } else { acceleration = stepper_acceleration; acceleration_sqrt = stepper_acceleration_sqrt; } if(stepper_deceleration == 0) { deceleration = 0xFFFF; } else { deceleration = stepper_deceleration; } if(stepper_velocity == 0) { delta = acceleration_sqrt; rest = 0; if(stepper_speedramp_state == STEPPER_SPEEDRAMP_STATE_FORWARD) { stepper_set_direction(STEPPER_DIRECTION_FORWARD); } else if(stepper_speedramp_state == STEPPER_SPEEDRAMP_STATE_BACKWARD) { stepper_set_direction(STEPPER_DIRECTION_BACKWARD); } } else { if(stepper_velocity < goal) { delta = (acceleration + rest) / stepper_velocity; rest = (acceleration + rest) % stepper_velocity; } else { delta = (deceleration + rest) / stepper_velocity; rest = (deceleration + rest) % stepper_velocity; } } if(stepper_velocity < goal) { stepper_velocity = MIN(stepper_velocity + delta, goal); } else { stepper_velocity = MAX(((int32_t)stepper_velocity) - delta, goal); } }
void stepper_make_step_speedramp(int32_t steps) { if(steps == 0) { stepper_position_reached = true; stepper_state = STEPPER_STATE_STOP; return; } if(steps < 0) { stepper_set_direction(STEPPER_DIRECTION_BACKWARD); steps = -steps; } else { stepper_set_direction(STEPPER_DIRECTION_FORWARD); } if(steps == 1) { // Just make the single step, no need for TC IRQ stepper_make_step(); stepper_position_reached = true; return; } uint16_t acceleration; uint16_t acceleration_sqrt; uint16_t deceleration; if(stepper_acceleration == 0) { acceleration = 0xFFFF; acceleration_sqrt = 256; // sqrt(0xFFFF) } else { acceleration = stepper_acceleration; acceleration_sqrt = stepper_acceleration_sqrt; } if(stepper_deceleration == 0) { deceleration = 0xFFFF; } else { deceleration = stepper_deceleration; } stepper_acceleration_steps = (stepper_velocity_goal*stepper_velocity_goal)/ (2*acceleration); if(stepper_acceleration_steps == 0) { stepper_acceleration_steps = 1; } int32_t acceleration_limit = (((int64_t)steps)*((int64_t)deceleration))/ (acceleration + deceleration); if(acceleration_limit == 0) { acceleration_limit = 1; } if(acceleration_limit <= stepper_acceleration_steps) { stepper_deceleration_steps = acceleration_limit - steps; } else { stepper_deceleration_steps = -(((int64_t)stepper_acceleration_steps) * ((int64_t)acceleration) / deceleration); } if(stepper_deceleration_steps == 0) { stepper_deceleration_steps = -1; } stepper_velocity = acceleration_sqrt; stepper_delay = VELOCITY_TO_DELAY(acceleration_sqrt); stepper_deceleration_start = steps + stepper_deceleration_steps; stepper_step_counter = 0; stepper_acceleration_counter = 0; stepper_speedramp_state = STEPPER_SPEEDRAMP_STATE_ACCELERATION; TC0_IrqHandler(); }