126 lines
3.5 KiB
C
126 lines
3.5 KiB
C
/*
|
|
* pse_stepper_planer.c
|
|
*
|
|
* Created on: Aug 14, 2023
|
|
* Author: leo
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
|
|
#include "lvgl.h"
|
|
|
|
#include "PSE_unit.h"
|
|
|
|
#include "pse_stepper_planer.h"
|
|
|
|
void pse_stepper_planer_tick(pse_unit* unit){
|
|
pse_stepper_conf* c = unit->stepper_conf;
|
|
|
|
HAL_GPIO_TogglePin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
|
|
c->steps_counter += (pse_sp_get_dir(c)?1:-1) * HAL_GPIO_ReadPin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
|
|
if(c->stopAtHome && c->steps_counter == 0){
|
|
pse_sp_jog_speed(c, 0);
|
|
pse_sp_stop_axis(c);
|
|
}
|
|
}
|
|
|
|
void pse_sp_start_axis(pse_stepper_conf* conf){
|
|
HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_RESET);
|
|
HAL_TIM_Base_Start_IT(conf->tim);
|
|
}
|
|
void pse_sp_stop_axis(pse_stepper_conf* conf){
|
|
HAL_TIM_Base_Stop_IT(conf->tim);
|
|
HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_SET);
|
|
}
|
|
|
|
void pse_sp_set_dir(pse_stepper_conf* conf, int dir){
|
|
HAL_GPIO_WritePin(conf->DIR_GPIO_Port, conf->DIR_GPIO_Pin, dir);
|
|
}
|
|
|
|
int pse_sp_get_dir(pse_stepper_conf* conf){
|
|
return HAL_GPIO_ReadPin(conf->DIR_GPIO_Port, conf->DIR_GPIO_Pin);
|
|
}
|
|
|
|
// https://en.wikipedia.org/wiki/Integer_square_root#Using_only_integer_division
|
|
static uint16_t int_sqrt(uint32_t s){
|
|
if (s <= 1)
|
|
return s;
|
|
|
|
uint32_t x0 = s / 2;
|
|
|
|
uint32_t x1 = (x0 + s / x0) / 2;
|
|
|
|
while (x1 < x0){
|
|
x0 = x1;
|
|
x1 = (x0 + s / x0) / 2;
|
|
}
|
|
return x0;
|
|
}
|
|
|
|
void pse_stepper_planer_compute_sps(pse_unit* unit){
|
|
pse_stepper_conf* c = unit->stepper_conf;
|
|
// magic constants are used to approximate pi and other multiplicatives constant as a rationnal
|
|
uint64_t numerator = (uint64_t)14323 * unit->syringe->diameter * unit->syringe->diameter * PSE_STEPPER_TIMER_CLOCK * PSE_STEPPER_SCREW_PITCH;
|
|
uint64_t denominator = (uint64_t)607887 * unit->flow * PSE_STEPPER_STEPS_PER_ROTATION;
|
|
uint64_t res = numerator / denominator;
|
|
if(res == 0){
|
|
lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Error", "Invalid configuration entered (period = 0)", NULL, true);
|
|
lv_obj_center(mbox1);
|
|
return;
|
|
}
|
|
else if(res >= ((uint64_t)1 << 32)){
|
|
lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Error", "Invalid configuration entered (period > 2^32)", NULL, true);
|
|
lv_obj_center(mbox1);
|
|
return;
|
|
}
|
|
|
|
if(res < 65536){
|
|
c->tim_presc = 0;
|
|
c->tim_period = res - 1;
|
|
__HAL_TIM_SET_AUTORELOAD(c->tim, c->tim_period);
|
|
__HAL_TIM_SET_PRESCALER(c->tim, c->tim_presc);
|
|
}
|
|
else{
|
|
uint16_t sqrt = int_sqrt(res);
|
|
c->tim_presc = sqrt - 1;
|
|
c->tim_period = sqrt;
|
|
__HAL_TIM_SET_AUTORELOAD(c->tim, c->tim_period);
|
|
__HAL_TIM_SET_PRESCALER(c->tim, c->tim_presc);
|
|
}
|
|
lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Info", "aaa", NULL, true);
|
|
lv_obj_t * mboxtxt = lv_msgbox_get_text(mbox1);
|
|
lv_label_set_text_fmt(mboxtxt, "res : %llu\npresc : %d\nperiod : %d", res, c->tim_presc, c->tim_period);
|
|
lv_obj_center(mbox1);
|
|
}
|
|
|
|
void pse_sp_set_dir_all(pse_unit* units, int units_num, int dir){
|
|
for(int i = 0; i < units_num; i++){
|
|
pse_sp_set_dir(units[i].stepper_conf, dir);
|
|
}
|
|
}
|
|
|
|
void pse_sp_start_all(pse_unit* units, int units_num){
|
|
for(int i = 0; i < units_num; i++){
|
|
if(units[i].enabled){
|
|
pse_sp_start_axis(units[i].stepper_conf);
|
|
}
|
|
}
|
|
}
|
|
void pse_sp_stop_all(pse_unit* units, int units_num){
|
|
for(int i = 0; i < units_num; i++){
|
|
if(units[i].enabled)
|
|
pse_sp_stop_axis(units[i].stepper_conf);
|
|
}
|
|
}
|
|
|
|
void pse_sp_jog_speed(pse_stepper_conf* conf, int status){
|
|
if(status){
|
|
__HAL_TIM_SET_AUTORELOAD(conf->tim, PSE_STEPPER_JOG_PERIOD);
|
|
__HAL_TIM_SET_PRESCALER(conf->tim, PSE_STEPPER_JOG_PRESC);
|
|
}
|
|
else{
|
|
__HAL_TIM_SET_AUTORELOAD(conf->tim, conf->tim_period);
|
|
__HAL_TIM_SET_PRESCALER(conf->tim, conf->tim_presc);
|
|
}
|
|
}
|