63f0da25f1
new driver works well while testing with one button with debug output - small changes to isr function that made it fully functional! - add stepper debug task - minor optimizations necessary
280 lines
9.2 KiB
C++
280 lines
9.2 KiB
C++
extern "C"
|
|
{
|
|
#include <stdio.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_log.h"
|
|
#include "driver/adc.h"
|
|
}
|
|
|
|
#include "stepper.hpp"
|
|
#include "config.hpp"
|
|
#include "guide-stepper.hpp"
|
|
#include "encoder.hpp"
|
|
|
|
|
|
|
|
//---------------------
|
|
//--- configuration ---
|
|
//---------------------
|
|
//also see config.hpp
|
|
//for pin definition
|
|
|
|
#define STEPPER_TEST_TRAVEL 65 //mm
|
|
//
|
|
#define MIN_MM 0
|
|
#define MAX_MM 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 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
|
|
|
|
|
|
|
|
//----------------------
|
|
//----- variables ------
|
|
//----------------------
|
|
static const char *TAG = "stepper"; //tag for logging
|
|
|
|
static bool stepp_direction = true;
|
|
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");
|
|
//}
|
|
|
|
|
|
//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
|
|
|
|
stepper_init();
|
|
}
|
|
|
|
|
|
//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)
|
|
{
|
|
stepper_init();
|
|
int state = 0;
|
|
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();
|
|
|
|
//cycle through test commands with one button
|
|
if (SW_RESET.risingEdge) {
|
|
switch (state){
|
|
case 0:
|
|
stepper_setTargetSteps(1000);
|
|
state++;
|
|
break;
|
|
case 1:
|
|
stepper_setTargetSteps(100);
|
|
state++;
|
|
break;
|
|
case 2:
|
|
stepper_setTargetSteps(2000);
|
|
state = 0;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
// if (SW_PRESET1.risingEdge) {
|
|
// buzzer.beep(2, 300, 100);
|
|
// stepperSw_setTargetSteps(1000);
|
|
// }
|
|
// if (SW_PRESET2.risingEdge) {
|
|
// buzzer.beep(1, 500, 100);
|
|
// stepperSw_setTargetSteps(10000);
|
|
// }
|
|
// if (SW_PRESET3.risingEdge) {
|
|
// buzzer.beep(1, 100, 100);
|
|
// stepperSw_setTargetSteps(30000);
|
|
// }
|
|
}
|
|
|
|
|
|
|
|
|
|
//----------------------------
|
|
//----- 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);
|
|
//
|
|
// //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);
|
|
// }
|
|
// }
|
|
}
|