void set_max_velocity(const ComType com, const SetMaxVelocity *data) {
uint32_t old_velocity_goal = stepper_velocity_goal;
stepper_velocity_goal = data->velocity;
if(stepper_state == STEPPER_STATE_DRIVE && old_velocity_goal == 0) {
TC0_IrqHandler();
}
com_return_setter(com, data);
}
void stepper_make_drive_speedramp(uint8_t state) {
stepper_state = STEPPER_STATE_DRIVE;
stepper_speedramp_state = state;
if(!stepper_is_currently_running()) {
// call drive speedramp one time, to get it going
// (i.e. make velocity != 0)
stepper_drive_speedramp();
TC0_IrqHandler();
}
}
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();
}
