/** 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();
}
