// Break down the distance into 1, 1/2, 1/4/, and 1/8 steps
uint8_t resolve_distance(enum step_size resolution, float dist_mm, uint8_t pitch){
float granularity = 1.8f*(1.0f/(float)resolution); // 1.8, 0.9, 0.45, or 0.225 degrees
float step_unit = ((granularity/360.0f)*(float)pitch); // factor by pitch
if((dist_mm / step_unit) >= 1.0f){ // at least 1 step can be performed at this resolution
uint32_t step_count = (dist_mm / step_unit);
float remaining_dist = dist_mm - (step_count * step_unit); // Deduct distance covered at this resolution
set_resolution(resolution);
perform_steps(step_count);
if(remaining_dist >= (float)(FINEST_STEP*pitch))
return resolve_distance((enum step_size)((uint8_t)resolution*2), remaining_dist, pitch);
} else if(dist_mm >= (float)(FINEST_STEP*pitch)){
// recursively call func with next finest resolution
return resolve_distance((enum step_size)((uint8_t)resolution*2), dist_mm, pitch);
}
if(dist_mm > 0)
return 0; // Return 0 if specified distance could not be fully resolved
else
return 1;
}
void LocalPlanner::move_to_point(){
get_current_location();
get_destination();
calculate_new_heading();
if (check_heading_difference() == 0)
resolve_heading();
else
resolve_distance();
}
void translate(enum stepper_motor motor, float dist_mm, uint8_t pitch, enum stepper_direction direction){
// Set direction
if(direction == STEPPER_CW){
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_14, GPIO_PIN_SET);
} else {
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_14, GPIO_PIN_RESET);
}
// Select stepper
switch(motor){
case STEPPER_PWDR_SRC:
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, GPIO_PIN_RESET);
break;
case STEPPER_PWDR_SINK:
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET);
break;
case STEPPER_ROLLER:
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_RESET);
break;
default:
break;
};
// Generate PWM
resolve_distance(STEPSZE_ONE, dist_mm, pitch);
// Reset stepper selection
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4, GPIO_PIN_SET);
// Reset stepper resolution
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5 | GPIO_PIN_6, GPIO_PIN_RESET);
}
