j/cable-length-cutter
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
cable-length-cutter/main/guide-stepper.cpp
jonny_l480 279ac0e07e Add more debug output ISSUES 
- Add debug output in calc
- disable test mode for testing with encoder
- set loglevel to DEBUG

extend feature currently only works well in testing mode with buttons
issues when running with encoder:
 movement gets extended extremely often due to encoder travel
 interval and rarely does reach target - compared to trigger with
 buttons
 -> while debugging noticed that the current speed gets negative
 and the xISR gets stuck so moves infinitely or not at all

 ideas:
 - rounding issue?
 - SPEED increment also has to be adjusted or set to 0 as step
   increment?
2023-03-26 11:41:46 +02:00

308 lines
10 KiB
C++
Raw Blame History

extern "C"
{
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/adc.h"
}
#include "DendoStepper.h"
#include "config.hpp"
#include "guide-stepper.hpp"
#include "encoder.hpp"
#include "gpio_evaluateSwitch.hpp"
//---------------------
//--- configuration ---
//---------------------
//also see config.hpp
//for pin definition
#define STEPPER_TEST_TRAVEL 65 //mm
//
#define MIN_MM 0
#define MAX_MM 110 //60
#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 SPEED 10 //35, 100, 50 rev
#define ACCEL_MS 800.0 //ms from 0 to max
#define DECEL_MS 500.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
//----------------------
//----- variables ------
//----------------------
static DendoStepper step;
static const char *TAG = "stepper"; //tag for logging
static bool stepp_direction = true;
static bool dir = true, dirPrev; //TODO local variables in travelSteps?
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){
//--- wait if direction changed ---
//if (dirPrev != dir){
// ESP_LOGW(TAG, " dir-change detected - waiting for move to finish \n ");
// while(step.getState() != 1) vTaskDelay(1); //wait for move to finish
//}
//--- 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
dirPrev = dir;
dir = 1;
//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
dirPrev = dir;
dir = 1;
//-- dont wait for move to finish since moves in same direction get merged --
//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);
dirPrev = dir;
dir = 0;
//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
dirPrev = dir;
dir = 0;
//-- dont wait for move to finish since moves in same direction get merged --
//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");
}
//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
}
//function that updates speed value using ADC input and configured MIN/MAX
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);
ESP_LOGW(TAG, "poti: %d (%.2lf%%), set speed to: %d", potiRead, poti*100, speed);
}
//----------------------------
//---- TASK stepper-test -----
//----------------------------
void task_stepper_test(void *pvParameter)
{
init_stepper();
home();
step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS);
//--- move from left to right repeatedly ---
// while (1) {
// updateSpeedFromAdc();
// step.runPosMm(STEPPER_TEST_TRAVEL);
// while(step.getState() != 1) vTaskDelay(2);
// ESP_LOGI(TAG, "finished moving right => moving left");
// 10updateSpeedFromAdc();
// step.runPosMm(-STEPPER_TEST_TRAVEL);
// while(step.getState() != 1) vTaskDelay(2); //1=idle
// ESP_LOGI(TAG, "finished moving left => moving right");
// }
//--- control stepper using preset buttons ---
while(1){
vTaskDelay(20 / portTICK_PERIOD_MS);
//------ handle switches ------
//run handle functions for all switches
SW_START.handle();
SW_RESET.handle();
SW_SET.handle();
SW_PRESET1.handle();
SW_PRESET2.handle();
SW_PRESET3.handle();
SW_CUT.handle();
SW_AUTO_CUT.handle();
if (SW_RESET.risingEdge) {
ESP_LOGI(TAG, "button - stop stepper\n ");
buzzer.beep(1, 1000, 100);
step.stop();
}
if (SW_PRESET1.risingEdge) {
ESP_LOGI(TAG, "button - moving right\n ");
buzzer.beep(2, 300, 100);
step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS);
step.runPosMm(15);
}
if (SW_PRESET3.risingEdge) {
ESP_LOGI(TAG, "button - moving left\n ");
buzzer.beep(1, 500, 100);
step.setSpeedMm(SPEED, ACCEL_MS, DECEL_MS);
step.runPosMm(-15);
}
if (SW_PRESET2.risingEdge) {
buzzer.beep(1, 100, 100);
ESP_LOGW(TAG, "button - current state: %d\n, pos: %llu", (int)step.getState(), step.getPositionMm());
}
}
}
//----------------------------
//----- TASK stepper-ctl -----
//----------------------------
void task_stepper_ctl(void *pvParameter)
{
//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);
ESP_LOGI(TAG, "delaying stepper-ctl task by %.1f ms (poti value)", 2000 * potiModifier);
vTaskDelay(2000 * potiModifier / portTICK_PERIOD_MS);
//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(SPEED, ACCEL_MS, DECEL_MS);
//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);
}
}
}
Reference in New Issue View Git Blame Copy Permalink