From 9cae5cf75d307c9a9e112edff78e4e0370cfeb0a Mon Sep 17 00:00:00 2001 From: jonny_ji7 Date: Wed, 26 Apr 2023 17:46:06 +0200 Subject: [PATCH] Move stepper-cfg to config.h, Rework guide-stepper.cpp (new driver) - move stepper config e.g. speed accel steps... from stepper.cpp to config.h - uncomment and rework code in guide-stepper.cpp (for traveling the stepper based on encoder) to work with new custom driver instead of previous library Note currently in STEPPER_SIMULATE_ECNODER mode (use button as encoder), see macro --- main/config.hpp | 14 +- main/guide-stepper.cpp | 295 ++++++++++++++++++++--------------------- main/stepper.cpp | 19 +-- 3 files changed, 164 insertions(+), 164 deletions(-) diff --git a/main/config.hpp b/main/config.hpp index 95ca28b..aa7921a 100644 --- a/main/config.hpp +++ b/main/config.hpp @@ -86,10 +86,16 @@ extern "C" { //-------------------------- //enable stepper test mode (dont start control and encoder task) #define STEPPER_TEST -#define STEPPER_STEP_PIN GPIO_NUM_18 //mos1 -#define STEPPER_DIR_PIN GPIO_NUM_16 //ST3 -#define STEPPER_EN_PIN GPIO_NUM_0 //not connected (-> stepper always on) -//more detailed options for testing are currently defined in guide-stepper.cpp +//pins +#define STEPPER_STEP_PIN GPIO_NUM_18 //mos1 +#define STEPPER_DIR_PIN GPIO_NUM_16 //ST3 +//driver settings +#define STEPPER_STEPS_PER_MM 200/2 //steps/mm (steps-per-rot / slope) +#define STEPPER_SPEED_DEFAULT 20 //mm/s +#define STEPPER_SPEED_MIN 4 //mm/s - speed threshold at which stepper immediately starts/stops +#define STEPPER_ACCEL_INC 3 //steps/s increment per cycle +#define STEPPER_DECEL_INC 8 //steps/s decrement per cycle +//options affecting movement are currently defined in guide-stepper.cpp diff --git a/main/guide-stepper.cpp b/main/guide-stepper.cpp index 87d7ffa..43d9ffd 100644 --- a/main/guide-stepper.cpp +++ b/main/guide-stepper.cpp @@ -21,32 +21,28 @@ extern "C" //for pin definition #define STEPPER_TEST_TRAVEL 65 //mm - // + #define MIN_MM 0 -#define MAX_MM 60 +#define MAX_MM 40 #define POS_MAX_STEPS MAX_MM * STEPPER_STEPS_PER_MM #define POS_MIN_STEPS MIN_MM * STEPPER_STEPS_PER_MM - #define SPEED_MIN 2.0 //mm/s #define SPEED_MAX 60.0 //mm/s -#define ACCEL_MS 100.0 //ms from 0 to max -#define DECEL_MS 90.0 //ms from max to 0 - -#define STEPPER_STEPS_PER_ROT 1600 -#define STEPPER_STEPS_PER_MM STEPPER_STEPS_PER_ROT/4 - #define D_CABLE 6 #define D_REEL 160 //actual 170 #define PI 3.14159 +//simulate encoder with reset button to test stepper ctl task +//note STEPPER_TEST has to be defined as well +#define STEPPER_SIMULATE_ENCODER //---------------------- //----- variables ------ //---------------------- -static const char *TAG = "stepper"; //tag for logging +static const char *TAG = "stepper-ctrl"; //tag for logging static bool stepp_direction = true; static uint32_t posNow = 0; @@ -56,97 +52,83 @@ static uint32_t posNow = 0; //---------------------- //----- functions ------ //---------------------- -////move axis certain Steps (relative) between left and right or reverse when negative -//void travelSteps(int stepsTarget){ -// //posNow = step.getPositionMm(); //not otherwise controlled, so no update necessary -// int stepsToGo, remaining; -// -// stepsToGo = abs(stepsTarget); -// if(stepsTarget < 0) stepp_direction = !stepp_direction; //invert direction in reverse mode -// -// while (stepsToGo != 0){ -// //--- currently moving right --- -// if (stepp_direction == true){ //currently moving right -// remaining = POS_MAX_STEPS - posNow; //calc remaining distance fom current position to limit -// if (stepsToGo > remaining){ //new distance will exceed limit -// //....step.runAbs (POS_MAX_STEPS); //move to limit -// //....while(step.getState() != 1) vTaskDelay(1); //wait for move to finish -// posNow = POS_MAX_STEPS; -// stepp_direction = false; //change current direction for next iteration -// stepsToGo = stepsToGo - remaining; //decrease target length by already traveled distance -// ESP_LOGI(TAG, " --- moved to max -> change direction (L) --- \n "); -// } -// else { //target distance does not reach the limit -// //....step.runAbs (posNow + stepsToGo); //move by (remaining) distance to reach target length -// //....while(step.getState() != 1) vTaskDelay(1); //wait for move to finish -// ESP_LOGD(TAG, "moving to %d\n", posNow+stepsToGo); -// posNow += stepsToGo; -// stepsToGo = 0; //finished, reset target length (could as well exit loop/break) -// } -// } -// -// //--- currently moving left --- -// else { -// remaining = posNow - POS_MIN_STEPS; -// if (stepsToGo > remaining){ -// //....step.runAbs (POS_MIN_STEPS); -// //....while(step.getState() != 1) vTaskDelay(2); //wait for move to finish -// posNow = POS_MIN_STEPS; -// stepp_direction = true; -// stepsToGo = stepsToGo - remaining; -// ESP_LOGI(TAG, " --- moved to min -> change direction (R) --- \n "); -// } -// else { -// //....step.runAbs (posNow - stepsToGo); //when moving left the coordinate has to be decreased -// while(step.getState() != 1) vTaskDelay(2); //wait for move to finish -// ESP_LOGD(TAG, "moving to %d\n", posNow - stepsToGo); -// posNow -= stepsToGo; -// stepsToGo = 0; -// } -// } -// } -// if(stepsTarget < 0) stepp_direction = !stepp_direction; //undo inversion of stepp_direction after reverse mode is finished -// return; -//} -// -// -////move axis certain Mm (relative) between left and right or reverse when negative -//void travelMm(int length){ -// travelSteps(length * STEPPER_STEPS_PER_MM); -//} -// -// -////define zero/start position -////currently crashes into hardware limitation for certain time -////TODO: limit switch -//void home() { -// ESP_LOGW(TAG, "auto-home..."); -// //....step.setSpeedMm(100, 500, 10); -// //....step.runInf(1); -// vTaskDelay(1500 / portTICK_PERIOD_MS); -// //....step.stop(); -// //....step.resetAbsolute(); -// ESP_LOGW(TAG, "auto-home finished"); -//} +//move axis certain Steps (relative) between left and right or reverse when negative +void travelSteps(int stepsTarget){ + //TODO simplify this function, one simple calculation of new position? + //with new custom driver no need to detect direction change + + int stepsToGo, remaining; + + stepsToGo = abs(stepsTarget); + if(stepsTarget < 0) stepp_direction = !stepp_direction; //invert direction in reverse mode + + while (stepsToGo != 0){ + //--- currently moving right --- + if (stepp_direction == true){ //currently moving right + remaining = POS_MAX_STEPS - posNow; //calc remaining distance fom current position to limit + if (stepsToGo > remaining){ //new distance will exceed limit + stepper_setTargetPosSteps(POS_MAX_STEPS); //move to limit + posNow = POS_MAX_STEPS; + stepp_direction = false; //change current direction for next iteration + stepsToGo = stepsToGo - remaining; //decrease target length by already traveled distance + ESP_LOGI(TAG, " --- moved to max -> change direction (L) --- \n "); + } + else { //target distance does not reach the limit + stepper_setTargetPosSteps(posNow + stepsToGo); //move by (remaining) distance to reach target length + ESP_LOGD(TAG, "moving to %d\n", posNow+stepsToGo); + posNow += stepsToGo; + stepsToGo = 0; //finished, reset target length (could as well exit loop/break) + } + } + + //--- currently moving left --- + else { + remaining = posNow - POS_MIN_STEPS; + if (stepsToGo > remaining){ + stepper_setTargetPosSteps(POS_MIN_STEPS); + posNow = POS_MIN_STEPS; + stepp_direction = true; + stepsToGo = stepsToGo - remaining; + ESP_LOGI(TAG, " --- moved to min -> change direction (R) --- \n "); + } + else { + stepper_setTargetPosSteps(posNow - stepsToGo); //when moving left the coordinate has to be decreased + ESP_LOGD(TAG, "moving to %d\n", posNow - stepsToGo); + posNow -= stepsToGo; + stepsToGo = 0; + } + } + } + if(stepsTarget < 0) stepp_direction = !stepp_direction; //undo inversion of stepp_direction after reverse mode is finished + return; +} + + +//move axis certain Mm (relative) between left and right or reverse when negative +void travelMm(int length){ + travelSteps(length * STEPPER_STEPS_PER_MM); +} + + +//define zero/start position +//currently crashes into hardware limitation for certain time +//TODO: limit switch +void home() { + //TODO add function for this to stepper driver + ESP_LOGW(TAG, "auto-home..."); + //....step.setSpeedMm(100, 500, 10); + //....step.runInf(1); + vTaskDelay(1500 / portTICK_PERIOD_MS); + //....step.stop(); + //....step.resetAbsolute(); + ESP_LOGW(TAG, "auto-home finished"); +} //initialize/configure stepper instance void init_stepper() { -// ESP_LOGW(TAG, "initializing stepper..."); -// DendoStepper_config_t step_cfg = { -// .stepPin = STEPPER_STEP_PIN, -// .dirPin = STEPPER_DIR_PIN, -// .enPin = STEPPER_EN_PIN, -// .timer_group = TIMER_GROUP_0, -// .timer_idx = TIMER_0, -// .miStep = MICROSTEP_32, -// .stepAngle = 1.8}; -// //....step.config(&step_cfg); -// //....step.init(); -// -// //....step.setSpeed(1000, 1000, 1000); //random default speed -// //....step.setStepsPerMm(STEPPER_STEPS_PER_MM); //guide: 4mm/rot - + //TODO unnecessary wrapper? + ESP_LOGW(TAG, "initializing stepper..."); stepper_init(); } @@ -156,7 +138,7 @@ void updateSpeedFromAdc() { int potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095 GPIO34 double poti = potiRead/4095.0; int speed = poti*(SPEED_MAX-SPEED_MIN) + SPEED_MIN; - //....step.setSpeedMm(speed, ACCEL_MS, DECEL_MS); + stepper_setSpeed(speed); ESP_LOGW(TAG, "poti: %d (%.2lf%%), set speed to: %d", potiRead, poti*100, speed); } @@ -165,6 +147,7 @@ void updateSpeedFromAdc() { //---------------------------- //---- TASK stepper-test ----- //---------------------------- +#ifndef STEPPER_SIMULATE_ENCODER void task_stepper_test(void *pvParameter) { stepper_init(); @@ -222,66 +205,74 @@ void task_stepper_test(void *pvParameter) // stepperSw_setTargetSteps(30000); // } } - +#endif //---------------------------- //----- TASK stepper-ctl ----- //---------------------------- +#ifdef STEPPER_SIMULATE_ENCODER +void task_stepper_test(void *pvParameter) +#else void task_stepper_ctl(void *pvParameter) +#endif { -// //variables -// int encStepsNow = 0; //get curret steps of encoder -// int encStepsPrev = 0; //steps at last check -// int encStepsDelta = 0; //steps changed since last iteration -// -// double cableLen = 0; -// double travelStepsExact = 0; //steps axis has to travel -// double travelStepsPartial = 0; -// int travelStepsFull = 0; -// double travelMm = 0; -// double turns = 0; -// -// float potiModifier; -// -// init_stepper(); -// home(); -// -// while(1){ -// //get current length -// encStepsNow = encoder_getSteps(); -// -// //calculate change -// encStepsDelta = encStepsNow - encStepsPrev; //FIXME MAJOR BUG: when resetting encoder/length in control task, diff will be huge! -// -// //read potentiometer and normalize (0-1) to get a variable for testing -// potiModifier = (float) gpio_readAdc(ADC_CHANNEL_POTI) / 4095; //0-4095 -> 0-1 -// //ESP_LOGI(TAG, "current poti-modifier = %f", potiModifier); -// -// //calculate steps to move -// cableLen = (double)encStepsDelta * 1000 / ENCODER_STEPS_PER_METER; -// turns = cableLen / (PI * D_REEL); -// travelMm = turns * D_CABLE; -// travelStepsExact = travelMm * STEPPER_STEPS_PER_MM + travelStepsPartial; //convert mm to steps and add not moved partial steps -// travelStepsPartial = 0; -// travelStepsFull = (int)travelStepsExact; -// -// //move axis when ready to move at least 1 step -// if (abs(travelStepsFull) > 1){ -// travelStepsPartial = fmod(travelStepsExact, 1); //save remaining partial steps to be added in the next iteration -// ESP_LOGD(TAG, "cablelen=%.2lf, turns=%.2lf, travelMm=%.3lf, travelStepsExact: %.3lf, travelStepsFull=%d, partialStep=%.3lf", cableLen, turns, travelMm, travelStepsExact, travelStepsFull, travelStepsPartial); -// ESP_LOGI(TAG, "MOVING %d steps", travelStepsFull); -// //TODO: calculate variable speed for smoother movement? for example intentionally lag behind and calculate speed according to buffered data -// //....step.setSpeedMm(35, 100, 50); -// //testing: get speed from poti -// //step.setSpeedMm(35, 1000*potiModifier+1, 1000*potiModifier+1); -// travelSteps(travelStepsExact); -// encStepsPrev = encStepsNow; //update previous length -// } -// else { -// //TODO use encoder queue to only run this check at encoder event? -// vTaskDelay(2); -// } -// } + //variables + int encStepsNow = 0; //get curret steps of encoder + int encStepsPrev = 0; //steps at last check + int encStepsDelta = 0; //steps changed since last iteration + + double cableLen = 0; + double travelStepsExact = 0; //steps axis has to travel + double travelStepsPartial = 0; + int travelStepsFull = 0; + double travelMm = 0; + double turns = 0; + + float potiModifier; + + init_stepper(); + home(); + + while(1){ +#ifdef STEPPER_SIMULATE_ENCODER + //TESTING - simulate encoder using switch + SW_RESET.handle(); + //note + if (SW_RESET.risingEdge) encStepsNow += 500; +#else + //get current length + encStepsNow = encoder_getSteps(); +#endif + + //calculate change + encStepsDelta = encStepsNow - encStepsPrev; //FIXME MAJOR BUG: when resetting encoder/length in control task, diff will be huge! + + //read potentiometer and normalize (0-1) to get a variable for testing + potiModifier = (float) gpio_readAdc(ADC_CHANNEL_POTI) / 4095; //0-4095 -> 0-1 + //ESP_LOGI(TAG, "current poti-modifier = %f", potiModifier); + + //calculate steps to move + cableLen = (double)encStepsDelta * 1000 / ENCODER_STEPS_PER_METER; + turns = cableLen / (PI * D_REEL); + travelMm = turns * D_CABLE; + travelStepsExact = travelMm * STEPPER_STEPS_PER_MM + travelStepsPartial; //convert mm to steps and add not moved partial steps + travelStepsPartial = 0; + travelStepsFull = (int)travelStepsExact; + + //move axis when ready to move at least 1 step + if (abs(travelStepsFull) > 1){ + travelStepsPartial = fmod(travelStepsExact, 1); //save remaining partial steps to be added in the next iteration + ESP_LOGD(TAG, "cablelen=%.2lf, turns=%.2lf, travelMm=%.3lf, travelStepsExact: %.3lf, travelStepsFull=%d, partialStep=%.3lf", cableLen, turns, travelMm, travelStepsExact, travelStepsFull, travelStepsPartial); + ESP_LOGI(TAG, "MOVING %d steps", travelStepsFull); + travelSteps(travelStepsExact); + encStepsPrev = encStepsNow; //update previous length + } + else { + //TODO use encoder queue to only run this check at encoder event? + vTaskDelay(2); + } + vTaskDelay(10 / portTICK_PERIOD_MS); + } } diff --git a/main/stepper.cpp b/main/stepper.cpp index b586324..3f9232c 100644 --- a/main/stepper.cpp +++ b/main/stepper.cpp @@ -10,16 +10,19 @@ extern "C" { #include "esp_log.h" } -//config from config.hpp + +//===================== +//=== configuration === +//===================== +//used macros from config.hpp: //#define STEPPER_STEP_PIN GPIO_NUM_18 //mos1 //#define STEPPER_DIR_PIN GPIO_NUM_16 //ST3 -#define STEPPER_STEPS_PER_MM 200/2 //steps/mm -#define STEPPER_SPEED_DEFAULT 20 //mm/s -#define STEPPER_SPEED_MIN 4 //mm/s - speed at which stepper immediately starts/stops -#define STEPPER_ACCEL_INC 3 //steps/s per cycle -#define STEPPER_DECEL_INC 8 //steps/s per cycle - +//#define STEPPER_STEPS_PER_MM 200/2 //steps/mm (steps-per-rot / slope) +//#define STEPPER_SPEED_DEFAULT 20 //mm/s +//#define STEPPER_SPEED_MIN 4 //mm/s - speed threshold at which stepper immediately starts/stops +//#define STEPPER_ACCEL_INC 3 //steps/s increment per cycle +//#define STEPPER_DECEL_INC 8 //steps/s decrement per cycle #define TIMER_F 1000000ULL #define TICK_PER_S TIMER_S @@ -30,7 +33,7 @@ extern "C" { //======================== //=== global variables === //======================== -static const char *TAG = "stepper-ctl"; //tag for logging +static const char *TAG = "stepper-driver"; //tag for logging bool direction = 1; bool directionTarget = 1;