Compare commits

..

36 Commits

Author SHA1 Message Date
jonny_l480
f2dc43d7cd Merge branch 'dev' - dynamic winding width 2024-04-04 22:34:14 +02:00
jonny
7acb5c213a CAD: Add assembly of all parts (for website)
Due to color changes of linked parts all files got changed
(meaning no actual modifications to parts was made)
2024-03-23 22:26:14 +01:00
jonny_l480
4112d8653c Add dynamic-winding-length depending on targetLength
When winding short lengths e.g. 5m it does not make sense
to use the entire winding width thus getting unnecessary
wide cable ring.

Determined some thresholds while testing and implemented
those to be applied at target length change automatically:

control:
    - update winding width each time target length changes

guide-stepper:
    - add function that returns dynamic winding width depending
      on passed target length according to fixed thresholds
2024-03-17 09:41:24 +01:00
jonny_l480
84c88e8f02 Fix deadlock in windingWidth, Handle 0 width, optimize UI
guide-stepper:
- fix deadlock when editing winding width
- fix behaivor when windingWidth is set to 0 (disable guide)

control:
- add dead time to prevent setTarget when exiting setWindingWidth with buttons
- fix typo and conditons
- increase max winding width
- optimize display text
2024-03-16 11:47:20 +01:00
jonny
c049da3583 Add button combination to set custom windingWidth with poti
control:
    - Add Button combination SET+PRESET1 to set winding width with potentiometer
    - Add case and display output in that state

config: move config macros from guide_stepper to common config

guide-stepper:
    - outsource config definitions
    - add functions to get and set variable winding width
    - add mutex
2024-03-15 09:49:28 +01:00
jonny
1d0ea6a5aa Merge branch 'dev' - cable-guide functional 2024-03-14 14:09:32 +01:00
jonny
d08b629a0f Rework README, Minor changes to function-diagram
- Create actual readme describing the project
- Add photo
- Change project name
- Minor change in function-diagram for section used at website
2024-03-14 14:01:24 +01:00
jonny
5cf4b31ff0 Add control-pcb schematic, Add screenshots modbus tests
- Add schematic from old pcb repo

- Add screenshots of successful tests made some time ago
with usb dongle and the VFD (currently not planned)
2024-03-13 16:40:47 +01:00
jonny
2ee07d677c Merge branch 'nvs' into dev - restore last pos works
Last position gets stored at shutdown, loaded at startup and used
to optimize initial auto-home.
2024-03-13 16:33:59 +01:00
jonny
530536f9a3 Update Connection-plan: Measure voltage, Details, Color
connection-plan:
    - Add supply voltage measurement (onboard)
    - re-wire blue led
    - add github link

    - optimize panel description
    - add stepper information, fix color

    - wiring plan: Add specifications, Add colorconnection-plan:
2024-03-13 16:31:07 +01:00
jonny_l480
9dfca6f6c3 Fix ENCODER_TEST, Calibrate length measurement
config:
    - calibrate length measurement

control:
    - Fix ENCODER_TEST
        - syntax error
        - Add beep at 0.5m
2024-03-13 14:56:27 +01:00
jonny
1dd8388769 Add task shutDownDetection, save last pos to nvs, optimize auto-home
shutdown.cpp/hpp:
    - repeatedly check supply voltage
    - save value to nvs when dropping below threshold
    - provide function to read lastPos from nvs

guide-stepper:
    - auto-home considers stored last position in nvs

main:
    - add new task shutDownDetection

config:
    - slightly increase stepper speed
2024-03-12 23:58:15 +01:00
jonny
cfd2458599 CAD: Add cable-reel_v2.2.2, Update axis_mounts
Currently used sturdier version of cable reel with
attachable counter piece, printed using PETG

Currently used Axis components:
Changed rolls and mount block

Details see changelog in FCStd files (param spreadsheet)
2024-03-11 11:35:34 +01:00
jonny
4aab1e1343 Outsource global variables, Optimize comments
- outsource global variables and objects
    - move from config.hpp to global.cpp / hpp
    - rename config.hpp, thus change all includes
- re-arrange includes -> in source files instead of header

- optimize / make function comment titles more consistent
- optimize comments overall

- add several temporary files to gitignore
- fix unused variable compiler warnings
2024-03-10 12:55:13 +01:00
jonny_l480
4c149a7743 Adjust parameters cable-guide
Adjust some parameters while testing cable guide with new Reel V2.2.2
Guides quite well with current setup, calculated diameter (layer heights)
matches quite good now
2024-03-09 22:26:26 +01:00
jonny
e65350f643 Add layer-count, variable diameter, Optimize comments
guide-stepper:
    - Add layer count when reversing axis
    - Adjust steps calculation considering (add layers to reel-dia)
    - optimize comments
control: optimize comments
main: optimize comments
2024-03-09 13:50:45 +01:00
jonny
989f9cce13 Fix Bug 'weird axis movement at reset'
- guide-stepper:
    - add queue to send commands to stepper-ctl task
    - add function that tells stepper task to move to zero
    - increase travel length full axis length 110

- control: move guide to zero at reset

- Cmake: enable colored output
2024-03-08 17:02:54 +01:00
jonny_l480
c42f5d5d7a CAD: Update reel v2.2 (printed)
- slight adjustments to parameters
-> printed this version in PETG
2023-12-11 20:01:54 +01:00
jonny_jr9
59707b5cf4 CAD: Add reel_v2.2 with interlocking counter-part
- Add new version of cable reel with second counter-part
  to prevent cable from falling off at long lengths
- Also update axis-mounts (modified some time ago)
2023-11-29 22:56:34 +01:00
jonny_l480
e2749cf2b9 Adjust travel length (max with current reel)
increase and test machine using max length possible with current reel
and "counter reel"
Worked quite well, see video made
2023-05-01 10:07:43 +02:00
jonny_l480
d206b53194 Stepper driver: Add overshoot
- prevent hard stops (no decel ramp) when target changes while
  decelerating already (overshoot and move back)

- optimize/adjust stepper-test mode, add macro for test with single switch

Note: tested the current state, works quite well. very rarely crashes
probably dude to race conditions in isr?
2023-04-28 12:41:06 +02:00
jonny_l480
f8b9e6c862 Add waitForStop func, wait at direction change
- Add waitForStop function to custom stepper driver
- use that function in guide-stepper to ensure stepper actually was at
  max/min position before dir change
2023-04-28 09:36:04 +02:00
jonny_ji7
17d8db2193 Add 'stepper_home' function, disable testing mode
- Add function stepper_home
- disable STEPPER_TESTING and STEPPER_SIMULATE_ENCODER for normal
  for testing in operation
2023-04-26 21:56:25 +02:00
jonny_ji7
9cae5cf75d 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
2023-04-26 17:46:06 +02:00
jonny_ji7
a22c60b817 Merge branch 'stepper_custom-driver' into dev
custom stepper driver successfully implemented,
will be used and continued in the further development
in dev branch
2023-04-25 19:30:26 +02:00
jonny_ji7
7eab4f8a8e Revert 8 commits (approach with stepper lib dropped)
Undo all custom changes to the DendoStepper library, since it will not
be used further. Custom implementation will be used in the future
(stepper_custom-driver branch)

Revert 279ac0e07e7578a86a6d13908e472bd4f379a62a
Revert 1c59e0097b3732d9b035881faa442349c50d4341
Revert 5d7c67ab9a2327adc49ea92cd55f9aa23a40b065
Revert 8d2428d28576e08da07af79659620376b1e4e964
Revert dc6deeb3d09628ac7f4f4fab256dc168bcbf835a
Revert 0a05340763c26be4735d1cf845b96150112b25ad
Revert 45409676a0b502c69b5bc26be7361bfa5a89ed61
Revert de42b6252ebec3558713cd39334548041654545f
2023-04-25 19:16:38 +02:00
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
jonny_l480
1c59e0097b Fix feature: extend movement works now! (test mode)
while currently in stepper test mode the extend feature works as
intended now.
you can trigger movement using the buttons and repeating
presses while still moving (coast, accel or decel) extends the movement
without stopping or accelerating again.

Note reversing the direction while still moving is not handled and results
in immediate direction change and distance that makes no sense.

also added alot of debug output and switched to debug level
2023-03-24 15:05:36 +01:00
jonny_l480
5d7c67ab9a stepper-test: Control stepper with buttons
now the stepper test task controls the stepper using button input
this makes debugging modifications to stepper library easier
2023-03-24 13:22:22 +01:00
jonny_l480
8d2428d285 Stepper: Add calc debug output ISSUES
- sometimes deadlock at direction change
    waits for idle state in runAbs
    function, but stepper is not really moving anymore  ISR does not change
    to idle or state changed afterwards?
- every EXTEND operation results in a motor stop thus when extending
    alot is stopped only ramping up and down
    when encoder stop after several extend attempts
    the stepper moves all the remaining steps successfully
2023-03-13 20:25:10 +01:00
jonny_ji7
dc6deeb3d0 Stepper: rework resume functionality 2023-03-13 20:02:22 +01:00
jonny_l480
0a05340763 revert mutex, fix state reset, logging, slower
stepsRemaining not necessary in ISR
comment out forced stop at runAbs
statusPrev not needed
TODO: ISR defines state every time, no need to adjust manually while running
adjust calc function to handle remaining steps

Test: broken - slow moves mork but when "extending" movement it just
vibrates
2023-03-13 18:26:50 +01:00
jonny_ji7
45409676a0 Stepper: add mutex, logging, delay, less steps
When testing last commit the stepper task crashed almost instantly and
moved randomly. Trying to fix/debug that with those changes
2023-03-13 11:12:42 +01:00
jonny_ji7
de42b6252e Stepper: Add resume/update functionality (WIP)
add way to extend current move operation without having to wait for IDLE
state (full decel time)
2023-03-12 13:06:30 +01:00
jonny_ji7
0ede3dbfa8 Bump ESP-IDF version
Project is compatible with 4.4.4
2023-02-22 17:30:04 +01:00
jonny_ji7
a89d3a1f8c Update Readme: Add install instructions 2023-02-21 21:55:51 +01:00
52 changed files with 2418 additions and 580 deletions

14
.gitignore vendored
View File

@ -1,15 +1,29 @@
# ESP-IDF default build directory # ESP-IDF default build directory
build build
sdkconfig.old
dependencies.lock
# VIM files # VIM files
*.swp *.swp
*.swo *.swo
**/.cache
# VS-code
settings.json
# drawio files # drawio files
*.bkp *.bkp
# freecad backup files # freecad backup files
*.FCStd1 *.FCStd1
*.FCBak
# stl files are usually temporary
*.stl
# kicad backup files # kicad backup files
pcb/*/*backups/ pcb/*/*backups/
# other
octave-workspace
del
screenshots

View File

@ -6,4 +6,4 @@ cmake_minimum_required(VERSION 3.5)
#set(EXTRA_COMPONENT_DIRS ../esp-idf-lib/components) #set(EXTRA_COMPONENT_DIRS ../esp-idf-lib/components)
include($ENV{IDF_PATH}/tools/cmake/project.cmake) include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(cable-length-cutting) project(cable-length-cutter)

104
README.md
View File

@ -1,18 +1,94 @@
# Hardware # Overview
## connection plan Firmware and documentation of a custom built machine that winds and cuts cable to certain lengths.
See [connection-plan.pdf](connection-plan.pdf) Extensive details about this project can be found on the website: [pfusch.zone/cable-length-cutter](https://pfusch.zone/cable-length-cutter)
<img src="img/cable-length-cutter.jpg" alt="Photo machine" style="width:60%;">
Photo of the built machine
## Current Features
- Measure length using rotary encoder
- Wind to set length while start-button is pressed
- Automatic wire cutter
- Control interface:
- 2x 7-Segment display showing lengths and notifications
- Buzzer for acoustic notifications
- 4 Buttons and Potentiometer for setting the target length
- Reset and Cut Button
- Stepper motor controlling a linear axis guiding the cable while winding
- Store last axis position at shutdown
# Usage
![Panel Layout](img/panel-layout.svg)
# Electrical Details
For detailed documentation of all electrical aspects, see [connection-plan.pdf](connection-plan.pdf).
# Printed Parts
All designed and 3D-printed parts can be found in the [cad/](cad/) folder. These parts were designed using FreeCAD.
# Components # Components
## rotary encoder LPD3806-600BM-G5-24C ### Custom pcb with ESP-32 microcontroller
- Pulses: 600 p/r (Single-phase 600 pulses /R,Two phase 4 frequency doubling to 2400 pulses) See [connection-plan.pdf](connection-plan.pdf)
- Power source: DC5-24V
- Shaft: 6*13mm/0.23*0.51"
- Size: 38*35.5mm/1.49*1.39"
- Output :AB 2phase output rectangular orthogonal pulse circuit, the output for the NPN open collector output type
- Maximum mechanical speed: 5000 R / min
- Response frequency: 0-20KHz
- Cable length: 1.5 meter
- size: http://domoticx.com/wp-content/uploads/2020/05/LPD3806-afmetingen.jpg
- Wires: Green = A phase, white = B phase, red = Vcc power +, black = V0
### Rotary encoder LPD3806-600BM-G5-24C
```
- Pulses: 600 p/r (Single-phase 600 pulses /R,Two phase 4 frequency doubling to 2400 pulses)
- Power source: DC5-24V
- Shaft: 6*13mm/0.23*0.51"
- Size: 38*35.5mm/1.49*1.39"
- Output :AB 2phase output rectangular orthogonal pulse circuit, the output for the NPN open collector output type
- Maximum mechanical speed: 5000 R / min
- Response frequency: 0-20KHz
- Cable length: 1.5 meter
- size: http://domoticx.com/wp-content/uploads/2020/05/LPD3806-afmetingen.jpg
- Wires: Green = A phase, white = B phase, red = Vcc power +, black = V0
```
### Variable Frequency Drive T13-400W-12-H
See [docs/vfd/](docs/vfd/)
### Stepper-driver TB6600
See [docs/stepper-driver_TB6600-Manual.pdf](docs/stepper-driver_TB6600-Manual.pdf)
### Linear Axis with Nema-12 28HB40 Stepper
See [connection-plan.pdf](connection-plan.pdf)
# Installation
For this project **ESP-IDF v4.4.4** is required (with other versions it might not compile)
```bash
#download esp-idf
yay -S esp-idf
#alternatively clone the esp-idf repository from github and do `git checkout release/4.4`
#run installation script in installed folder
/opt/esp-idf/install.sh
```
# Build
### Set up environment
```bash
source /opt/esp-idf/export.sh
```
(run once per terminal)
### Compile
```bash
cd board_single
idf.py build
```
# Flash
- connect FTDI programmer to board (VCC to VCC; TX to RX; RX to TX)
- press REST and BOOT button
- release RESET button (keep pressing boot)
- run flash command:
```bash
idf.py flash
```

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -116,21 +116,35 @@ timer_avail:
esp_err_t DendoStepper::runPos(int32_t relative) esp_err_t DendoStepper::runPos(int32_t relative)
{ {
if (!relative) // why would u call it with 0 wtf //TODO only enable when actually moving
return ESP_ERR_NOT_SUPPORTED;
if (ctrl.status > IDLE)
{ // we are running, we need to adjust steps accordingly, for now just stop the movement
STEP_LOGW("DendoStepper", "Finising previous move, this command will be ignored");
return ESP_ERR_NOT_SUPPORTED;
}
if (ctrl.status == DISABLED) // if motor is disabled, enable it if (ctrl.status == DISABLED) // if motor is disabled, enable it
enableMotor(); enableMotor();
if (!relative) // why would u call it with 0 wtf
return ESP_ERR_NOT_SUPPORTED;
if (ctrl.status > IDLE) { //currently moving
bool newDir = (relative < 0); // CCW if <0, else set CW
if (ctrl.dir == newDir){ //current direction is the same
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = ctrl.status==COAST ? COAST : ACC; //stay at coast otherwise switch to ACC
calc(abs(relative + ctrl.stepsRemaining)); //calculate new velolcity profile for new+remaining steps
} else { //direction has changed
//direction change not supported TODO wait for decel finish / queue?
STEP_LOGW("DendoStepper", "DIRECTION HOT-CHANGE NOT SUPPORTED - Finising previous move, this command will be ignored");
return ESP_ERR_NOT_SUPPORTED;
}
}
else { //current state is IDLE
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = ACC; ctrl.status = ACC;
setDir(relative < 0); // set CCW if <0, else set CW setDir(relative < 0); // set CCW if <0, else set CW
currentPos += relative;
calc(abs(relative)); // calculate velocity profile calc(abs(relative)); // calculate velocity profile
}
currentPos += relative; //(target position / not actual)
ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval ESP_ERROR_CHECK(timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval)); // set HW timer alarm to stepinterval
//TODO timer has to be stopped before update if already running?
ESP_ERROR_CHECK(timer_start(conf.timer_group, conf.timer_idx)); // start the timer ESP_ERROR_CHECK(timer_start(conf.timer_group, conf.timer_idx)); // start the timer
return ESP_OK; return ESP_OK;
@ -195,6 +209,37 @@ void DendoStepper::setSpeedMm(uint32_t speed, uint16_t accT, uint16_t decT)
STEP_LOGI("DendoStepper", "Speed set: v=%d mm/s t+=%d s t-=%d s", speed, accT, decT); STEP_LOGI("DendoStepper", "Speed set: v=%d mm/s t+=%d s t-=%d s", speed, accT, decT);
} }
//CUSTOM - change speed while running
//FIXME: this approach does not work, since calc function would have to be run after change, this will mess up target steps...
//void DendoStepper::changeSpeed(uint32_t speed)
//{
// //TODO reduce duplicate code (e.g. call setSpeed function)
// //change speed
// ctrl.speed = speed;
// //change status to ACC/DEC
// STEP_LOGI("DendoStepper", "Speed changed: from v=%.2f rad/s to v=%.2f rad/s", ctrl.speed, speed);
// if (speed > ctrl.speed) ctrl.status = ACC;
// if (speed < ctrl.speed) ctrl.status = DEC;
//}
//
//void DendoStepper::changeSpeedMm(uint32_t speed)
//{
// //TODO reduce duplicate code (e.g. call setSpeedMm function)
// if (ctrl.stepsPerMm == 0)
// {
// STEP_LOGE("DendoStepper", "Steps per millimeter not set, cannot set the speed!");
// }
// //calc new speed
// float speedNew = speed * ctrl.stepsPerMm;
// //change status to ACC/DEC
// if (speedNew > ctl.speed) ctrl.status = ACC;
// if (speedNew < ctl.speed) ctrl.status = DEC;
// //update speed, log output
// ctrl.speed = speedNew;
// STEP_LOGI("DendoStepper", "Speed changed: from v=%.2f rad/s to v=%.2f rad/s", ctrl.speed, speedNew);
//}
void DendoStepper::setStepsPerMm(uint16_t steps) void DendoStepper::setStepsPerMm(uint16_t steps)
{ {
ctrl.stepsPerMm = steps; ctrl.stepsPerMm = steps;
@ -261,6 +306,7 @@ void DendoStepper::stop()
} }
ctrl.runInfinite = false; ctrl.runInfinite = false;
timer_pause(conf.timer_group, conf.timer_idx); // stop the timer timer_pause(conf.timer_group, conf.timer_idx); // stop the timer
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = IDLE; ctrl.status = IDLE;
ctrl.stepCnt = 0; ctrl.stepCnt = 0;
gpio_set_level((gpio_num_t)conf.stepPin, 0); gpio_set_level((gpio_num_t)conf.stepPin, 0);
@ -287,10 +333,20 @@ bool DendoStepper::xISR()
ctrl.stepCnt++; ctrl.stepCnt++;
//CUSTOM: track actual precice current position
if (ctrl.dir) {
ctrl.posActual ++;
} else {
ctrl.posActual --;
}
//CUSTOM: track remaining steps for eventually resuming
ctrl.stepsRemaining = ctrl.stepCnt - ctrl.stepCnt;
// we are done // we are done
if (ctrl.stepsToGo == ctrl.stepCnt && !ctrl.runInfinite) if (ctrl.stepsToGo == ctrl.stepCnt && !ctrl.runInfinite)
{ {
timer_pause(conf.timer_group, conf.timer_idx); // stop the timer timer_pause(conf.timer_group, conf.timer_idx); // stop the timer
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = IDLE; ctrl.status = IDLE;
ctrl.stepCnt = 0; ctrl.stepCnt = 0;
return 0; return 0;
@ -299,16 +355,19 @@ bool DendoStepper::xISR()
if (ctrl.stepCnt > 0 && ctrl.stepCnt < ctrl.accEnd) if (ctrl.stepCnt > 0 && ctrl.stepCnt < ctrl.accEnd)
{ // we are accelerating { // we are accelerating
ctrl.currentSpeed += ctrl.accInc; ctrl.currentSpeed += ctrl.accInc;
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = ACC; // we are accelerating, note that*/ ctrl.status = ACC; // we are accelerating, note that*/
} }
else if (ctrl.stepCnt > ctrl.coastEnd && !ctrl.runInfinite) else if (ctrl.stepCnt > ctrl.coastEnd && !ctrl.runInfinite)
{ // we must be deccelerating then { // we must be deccelerating then
ctrl.currentSpeed -= ctrl.decInc; ctrl.currentSpeed -= ctrl.decInc;
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = DEC; // we are deccelerating ctrl.status = DEC; // we are deccelerating
} }
else else
{ {
ctrl.currentSpeed = ctrl.targetSpeed; ctrl.currentSpeed = ctrl.targetSpeed;
ctrl.statusPrev = ctrl.status; //update previous status
ctrl.status = COAST; // we are coasting ctrl.status = COAST; // we are coasting
} }
@ -316,15 +375,18 @@ bool DendoStepper::xISR()
// set alarm to calculated interval and disable pin // set alarm to calculated interval and disable pin
GPIO.out_w1tc = (1ULL << conf.stepPin); GPIO.out_w1tc = (1ULL << conf.stepPin);
timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval); timer_set_alarm_value(conf.timer_group, conf.timer_idx, ctrl.stepInterval);
ctrl.stepCnt++;
return 1; return 1;
} }
void DendoStepper::calc(uint32_t targetSteps) void DendoStepper::calc(uint32_t targetSteps)
{ {
//steps from ctrl.speed -> 0:
ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc);
ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec); ctrl.decSteps = 0.5 * ctrl.dec * (ctrl.speed / ctrl.dec) * (ctrl.speed / ctrl.dec);
//steps from 0 -> ctrl.speed:
//ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc);
//steps from ctrl.currentSpeed -> ctrl.speed:
ctrl.accSteps = 0.5 * ctrl.acc * (ctrl.speed / ctrl.acc) * (ctrl.speed / ctrl.acc) * (ctrl.speed - ctrl.currentSpeed) / ctrl.speed;
if (targetSteps < (ctrl.decSteps + ctrl.accSteps)) if (targetSteps < (ctrl.decSteps + ctrl.accSteps))
{ {

View File

@ -94,6 +94,9 @@ typedef struct
float dec = 100; // decceleration in rad*second^-2 float dec = 100; // decceleration in rad*second^-2
uint32_t accSteps = 0; uint32_t accSteps = 0;
uint32_t decSteps = 0; uint32_t decSteps = 0;
int32_t stepsRemaining = 0;
uint64_t posActual = 0; //actual current pos incremented at every step
uint8_t statusPrev = DISABLED; //FIXME currently unused
uint8_t status = DISABLED; uint8_t status = DISABLED;
bool dir = CW; bool dir = CW;
bool runInfinite = false; bool runInfinite = false;
@ -208,6 +211,9 @@ public:
*/ */
void setSpeedMm(uint32_t speed, uint16_t accT, uint16_t decT); void setSpeedMm(uint32_t speed, uint16_t accT, uint16_t decT);
//CUSTOM: change speed while running
void changeSpeedMm(uint32_t speed);
/** /**
* @brief Set steps per 1 mm of linear movement * @brief Set steps per 1 mm of linear movement
* *

File diff suppressed because one or more lines are too long

Binary file not shown.

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 144 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 128 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 144 KiB

View File

@ -1,6 +1,6 @@
<mxfile host="Electron" modified="2023-04-24T10:25:15.632Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/21.1.2 Chrome/106.0.5249.199 Electron/21.4.3 Safari/537.36" etag="MGrY-6I6opffbVIVdFHA" version="21.1.2" type="device" pages="2"> <mxfile host="Electron" modified="2024-03-14T10:08:17.453Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/23.1.5 Chrome/120.0.6099.109 Electron/28.2.5 Safari/537.36" etag="IGTPBlitl9d9z0sPWQV8" version="23.1.5" type="device" pages="2">
<diagram id="pYACIs8yIpksGCPkkI0E" name="control, cutter, buttons, encoder"> <diagram id="pYACIs8yIpksGCPkkI0E" name="control, cutter, buttons, encoder">
<mxGraphModel dx="1240" dy="730" grid="1" gridSize="5" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="1654" math="0" shadow="0"> <mxGraphModel dx="1194" dy="741" grid="1" gridSize="5" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="1654" math="0" shadow="0">
<root> <root>
<mxCell id="0" /> <mxCell id="0" />
<mxCell id="1" parent="0" /> <mxCell id="1" parent="0" />
@ -25,7 +25,7 @@
<mxCell id="Jo9Ue6hLqtK935TEgpBK-28" value="SW_RESET.state" style="rhombus;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-22" vertex="1"> <mxCell id="Jo9Ue6hLqtK935TEgpBK-28" value="SW_RESET.state" style="rhombus;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-22" vertex="1">
<mxGeometry x="120" y="110" width="120" height="70" as="geometry" /> <mxGeometry x="120" y="110" width="120" height="70" as="geometry" />
</mxCell> </mxCell>
<mxCell id="Jo9Ue6hLqtK935TEgpBK-29" value="// lengthNow = 0&lt;br&gt;encoder_reset()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-22" vertex="1"> <mxCell id="Jo9Ue6hLqtK935TEgpBK-29" value="// lengthNow = 0&lt;br&gt;encoder_reset()&lt;br&gt;guide_moveToZero()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-22" vertex="1">
<mxGeometry x="120" y="240" width="120" height="60" as="geometry" /> <mxGeometry x="120" y="240" width="120" height="60" as="geometry" />
</mxCell> </mxCell>
<mxCell id="Jo9Ue6hLqtK935TEgpBK-30" value="true" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-22" source="Jo9Ue6hLqtK935TEgpBK-28" target="Jo9Ue6hLqtK935TEgpBK-29" edge="1"> <mxCell id="Jo9Ue6hLqtK935TEgpBK-30" value="true" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-22" source="Jo9Ue6hLqtK935TEgpBK-28" target="Jo9Ue6hLqtK935TEgpBK-29" edge="1">
@ -205,7 +205,7 @@
</mxGeometry> </mxGeometry>
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-2" value="autoCutEnabled == true" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Jo9Ue6hLqtK935TEgpBK-20" target="1-PaxUM0a53oYDua-TxR-1" edge="1"> <mxCell id="1-PaxUM0a53oYDua-TxR-2" value="autoCutEnabled == true" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.75;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Jo9Ue6hLqtK935TEgpBK-20" target="1-PaxUM0a53oYDua-TxR-1" edge="1">
<mxGeometry x="-0.4778" y="6" relative="1" as="geometry"> <mxGeometry x="-0.6954" y="6" relative="1" as="geometry">
<Array as="points"> <Array as="points">
<mxPoint x="299" y="1100" /> <mxPoint x="299" y="1100" />
<mxPoint x="267" y="1100" /> <mxPoint x="267" y="1100" />
@ -230,11 +230,11 @@
<mxCell id="8zFeNdrfwlNlW0LlzPxK-13" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;startArrow=none;startFill=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Jo9Ue6hLqtK935TEgpBK-19" target="Avus2w3zFXZjXSa-NAtZ-22" edge="1"> <mxCell id="8zFeNdrfwlNlW0LlzPxK-13" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;startArrow=none;startFill=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Jo9Ue6hLqtK935TEgpBK-19" target="Avus2w3zFXZjXSa-NAtZ-22" edge="1">
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry" />
</mxCell> </mxCell>
<mxCell id="8zFeNdrfwlNlW0LlzPxK-14" value="wind at dynamic speed&lt;br&gt;(slower speed when closer to target)" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="8zFeNdrfwlNlW0LlzPxK-14" value="wind at dynamic speed&lt;br&gt;(slower speed when closer to target)" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=none;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="67" y="839" width="125" height="50" as="geometry" /> <mxGeometry x="67" y="824" width="125" height="50" as="geometry" />
</mxCell> </mxCell>
<mxCell id="E9gbP4vXqY4O4zoDVCtS-5" value="&lt;font color=&quot;#990000&quot;&gt;todo: &lt;br&gt;&lt;div style=&quot;&quot;&gt;- reverse when target exceeded?&lt;/div&gt;&lt;br&gt;&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=left;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="E9gbP4vXqY4O4zoDVCtS-5" value="&lt;font color=&quot;#990000&quot;&gt;todo:&amp;nbsp;&lt;/font&gt;&lt;span style=&quot;color: rgb(153, 0, 0);&quot;&gt;reverse when target exceeded?&lt;/span&gt;&lt;font color=&quot;#990000&quot;&gt;&lt;br&gt;&lt;br&gt;&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=left;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=none;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="352.5" y="976.5" width="182.5" height="45" as="geometry" /> <mxGeometry x="373.75" y="976.5" width="206.25" height="45" as="geometry" />
</mxCell> </mxCell>
<mxCell id="E9gbP4vXqY4O4zoDVCtS-9" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fontColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="E9gbP4vXqY4O4zoDVCtS-8" target="Jo9Ue6hLqtK935TEgpBK-18" edge="1"> <mxCell id="E9gbP4vXqY4O4zoDVCtS-9" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fontColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="E9gbP4vXqY4O4zoDVCtS-8" target="Jo9Ue6hLqtK935TEgpBK-18" edge="1">
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry" />
@ -348,13 +348,13 @@
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry" />
</mxCell> </mxCell>
<mxCell id="Avus2w3zFXZjXSa-NAtZ-22" value="checkStopCondition()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="Avus2w3zFXZjXSa-NAtZ-22" value="checkStopCondition()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="405" y="836.5" width="130" height="25" as="geometry" /> <mxGeometry x="401.25" y="836.5" width="130" height="25" as="geometry" />
</mxCell> </mxCell>
<mxCell id="Jo9Ue6hLqtK935TEgpBK-25" value="&lt;font color=&quot;#0000ff&quot;&gt;start counting (no motor)&lt;br&gt;(pull and attach wire manually to spool)&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="Jo9Ue6hLqtK935TEgpBK-25" value="&lt;font color=&quot;#0000ff&quot;&gt;start counting (no motor)&lt;br&gt;(pull and attach wire manually to spool)&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;labelBackgroundColor=none;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="53.5" y="390" width="138.5" height="78.5" as="geometry" /> <mxGeometry x="53.5" y="390" width="138.5" height="78.5" as="geometry" />
</mxCell> </mxCell>
<mxCell id="8zFeNdrfwlNlW0LlzPxK-15" value="start or resume slow for some time&lt;br&gt;(time for verifying it spools correctly)" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="8zFeNdrfwlNlW0LlzPxK-15" value="start or resume slow for some time&lt;br&gt;(time for verifying it winds correctly)" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=none;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="63.75" y="649" width="109.5" height="50" as="geometry" /> <mxGeometry x="75" y="644" width="109.5" height="56" as="geometry" />
</mxCell> </mxCell>
<mxCell id="Avus2w3zFXZjXSa-NAtZ-34" value="&lt;font color=&quot;#0000ff&quot;&gt;manually control the motor via preset buttons and potentiometer&lt;br&gt;&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="Avus2w3zFXZjXSa-NAtZ-34" value="&lt;font color=&quot;#0000ff&quot;&gt;manually control the motor via preset buttons and potentiometer&lt;br&gt;&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="817.75" y="648.25" width="138.5" height="78.5" as="geometry" /> <mxGeometry x="817.75" y="648.25" width="138.5" height="78.5" as="geometry" />
@ -362,24 +362,6 @@
<mxCell id="aTO7ynV6Y0lAHxoPAONt-5" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="aTO7ynV6Y0lAHxoPAONt-5" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="696.75" y="535" width="79" height="28.5" as="geometry" /> <mxGeometry x="696.75" y="535" width="79" height="28.5" as="geometry" />
</mxCell> </mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-12" value="WINDING" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="aTO7ynV6Y0lAHxoPAONt-11" target="Avus2w3zFXZjXSa-NAtZ-22" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-11" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="430.5" y="774" width="79" height="28.5" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-14" value="WINDING_START" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="aTO7ynV6Y0lAHxoPAONt-13" target="Avus2w3zFXZjXSa-NAtZ-16" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-13" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="430.5" y="595" width="79" height="28.5" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-16" value="COUNTING" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="aTO7ynV6Y0lAHxoPAONt-15" target="Avus2w3zFXZjXSa-NAtZ-4" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-15" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="429" y="390" width="79" height="28.5" as="geometry" />
</mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-17" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="aTO7ynV6Y0lAHxoPAONt-17" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="945.37" y="430" width="79" height="28.5" as="geometry" /> <mxGeometry x="945.37" y="430" width="79" height="28.5" as="geometry" />
</mxCell> </mxCell>
@ -417,15 +399,15 @@
<mxPoint x="265" y="960" as="sourcePoint" /> <mxPoint x="265" y="960" as="sourcePoint" />
<mxPoint x="315" y="910" as="targetPoint" /> <mxPoint x="315" y="910" as="targetPoint" />
<Array as="points"> <Array as="points">
<mxPoint x="438" y="875" /> <mxPoint x="434" y="875" />
<mxPoint x="360" y="875" /> <mxPoint x="360" y="875" />
<mxPoint x="360" y="900" /> <mxPoint x="360" y="915" />
<mxPoint x="263" y="900" /> <mxPoint x="263" y="915" />
</Array> </Array>
</mxGeometry> </mxGeometry>
</mxCell> </mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-55" value="setDynSpeedLvl()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="aTO7ynV6Y0lAHxoPAONt-55" value="setDynSpeedLvl()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="403.5" y="893" width="130" height="25" as="geometry" /> <mxGeometry x="401.25" y="882.5" width="130" height="25" as="geometry" />
</mxCell> </mxCell>
<mxCell id="8zFeNdrfwlNlW0LlzPxK-2" value="lengthRemaining" style="rhombus;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="8zFeNdrfwlNlW0LlzPxK-2" value="lengthRemaining" style="rhombus;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="736.88" y="105" width="130" height="60" as="geometry" /> <mxGeometry x="736.88" y="105" width="130" height="60" as="geometry" />
@ -501,8 +483,8 @@
<mxCell id="aTO7ynV6Y0lAHxoPAONt-63" value="setDynSpeedLvl(1)" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="aTO7ynV6Y0lAHxoPAONt-63" value="setDynSpeedLvl(1)" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="403.5" y="704" width="130" height="25" as="geometry" /> <mxGeometry x="403.5" y="704" width="130" height="25" as="geometry" />
</mxCell> </mxCell>
<mxCell id="aTO7ynV6Y0lAHxoPAONt-66" value="limit speed lvl to 1&lt;br style=&quot;font-size: 9px;&quot;&gt;can be slower if close though" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;fontSize=9;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="aTO7ynV6Y0lAHxoPAONt-66" value="limit speed lvl to 1&lt;br style=&quot;font-size: 8px;&quot;&gt;can be slower if close though" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=none;fontSize=8;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="533.5" y="704" width="55" height="20" as="geometry" /> <mxGeometry x="334.5" y="704" width="66.75" height="25" as="geometry" />
</mxCell> </mxCell>
<mxCell id="mQTKtX7dmJeWt0bXEoog-30" value="&lt;span style=&quot;color: rgb(0, 0, 0); font-family: Helvetica; font-size: 11px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; background-color: rgb(255, 255, 255); text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&quot;&gt;cutter_isRunning() == false&lt;/span&gt;" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;endArrow=classic;endFill=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="mQTKtX7dmJeWt0bXEoog-1" target="mQTKtX7dmJeWt0bXEoog-29" edge="1"> <mxCell id="mQTKtX7dmJeWt0bXEoog-30" value="&lt;span style=&quot;color: rgb(0, 0, 0); font-family: Helvetica; font-size: 11px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; background-color: rgb(255, 255, 255); text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;&quot;&gt;cutter_isRunning() == false&lt;/span&gt;" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;endArrow=classic;endFill=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="mQTKtX7dmJeWt0bXEoog-1" target="mQTKtX7dmJeWt0bXEoog-29" edge="1">
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry" />
@ -516,14 +498,8 @@
<mxCell id="mQTKtX7dmJeWt0bXEoog-4" value="cutter_start()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="mQTKtX7dmJeWt0bXEoog-4" value="cutter_start()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="199.5" y="1255" width="135" height="30" as="geometry" /> <mxGeometry x="199.5" y="1255" width="135" height="30" as="geometry" />
</mxCell> </mxCell>
<mxCell id="mQTKtX7dmJeWt0bXEoog-9" value="&lt;font color=&quot;#990000&quot;&gt;todo: &lt;br&gt;&lt;/font&gt;&lt;span style=&quot;color: rgb(153, 0, 0);&quot;&gt;- ability to CANCEL cutting&lt;/span&gt;&lt;font color=&quot;#990000&quot;&gt;&lt;br&gt;- e.g.&amp;nbsp;&lt;/font&gt;&lt;span style=&quot;color: rgb(153, 0, 0);&quot;&gt;cancel when buttons released?&lt;/span&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=left;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="mQTKtX7dmJeWt0bXEoog-29" value="lengthNow = 0&lt;br&gt;guide_moveToZero()" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="349.75" y="1320" width="229" height="45" as="geometry" /> <mxGeometry x="206.75" y="1435" width="120.5" height="40" as="geometry" />
</mxCell>
<mxCell id="mQTKtX7dmJeWt0bXEoog-28" value="&lt;font color=&quot;#990000&quot;&gt;todo:require reset switch to be able to start again?&lt;br&gt;&lt;/font&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=left;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontColor=#0000FF;labelBackgroundColor=#FFFFFF;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="77" y="1115" width="91.5" height="45" as="geometry" />
</mxCell>
<mxCell id="mQTKtX7dmJeWt0bXEoog-29" value="lengthNow = 0" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="224.5" y="1430" width="85" height="30" as="geometry" />
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-4" value="delay passed" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="1-PaxUM0a53oYDua-TxR-1" target="mQTKtX7dmJeWt0bXEoog-4" edge="1"> <mxCell id="1-PaxUM0a53oYDua-TxR-4" value="delay passed" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="1-PaxUM0a53oYDua-TxR-1" target="mQTKtX7dmJeWt0bXEoog-4" edge="1">
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry" />
@ -534,14 +510,62 @@
<mxCell id="1-PaxUM0a53oYDua-TxR-1" value="state:&lt;br&gt;AUTO_CUT_WAITING" style="rounded=0;whiteSpace=wrap;html=1;fontStyle=1" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="1-PaxUM0a53oYDua-TxR-1" value="state:&lt;br&gt;AUTO_CUT_WAITING" style="rounded=0;whiteSpace=wrap;html=1;fontStyle=1" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="195.75" y="1135" width="142.5" height="60" as="geometry" /> <mxGeometry x="195.75" y="1135" width="142.5" height="60" as="geometry" />
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-3" value="indicate cut pending on display&lt;br&gt;beep countdown&lt;br&gt;blink lamp" style="rounded=1;whiteSpace=wrap;html=1;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="1-PaxUM0a53oYDua-TxR-3" value="indicate cut pending on display&lt;br&gt;beep countdown&lt;br&gt;blink lamp" style="rounded=1;whiteSpace=wrap;html=1;align=center;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1">
<mxGeometry x="395" y="1142.5" width="185" height="45" as="geometry" /> <mxGeometry x="395" y="1142.5" width="185" height="45" as="geometry" />
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-7" value="AUTO_CUT_WAITING" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="E9gbP4vXqY4O4zoDVCtS-33" source="1-PaxUM0a53oYDua-TxR-6" target="1-PaxUM0a53oYDua-TxR-3" edge="1"> <mxCell id="u4D9dyohbDwP2OhtFbo0-1" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=1.008;entryY=0.119;entryDx=0;entryDy=0;entryPerimeter=0;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="aTO7ynV6Y0lAHxoPAONt-63" target="Jo9Ue6hLqtK935TEgpBK-18">
<mxGeometry relative="1" as="geometry" /> <mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="469" y="745" />
<mxPoint x="555" y="745" />
<mxPoint x="555" y="651" />
</Array>
</mxGeometry>
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-6" value="main loop" style="shape=hexagon;perimeter=hexagonPerimeter2;whiteSpace=wrap;html=1;fixedSize=1;fillColor=#f8cecc;strokeColor=#b85450;" parent="E9gbP4vXqY4O4zoDVCtS-33" vertex="1"> <mxCell id="u4D9dyohbDwP2OhtFbo0-2" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=1.004;entryY=0.1;entryDx=0;entryDy=0;entryPerimeter=0;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Avus2w3zFXZjXSa-NAtZ-4" target="Jo9Ue6hLqtK935TEgpBK-21">
<mxGeometry x="448" y="1080" width="79" height="28.5" as="geometry" /> <mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="469" y="497" />
<mxPoint x="545" y="497" />
<mxPoint x="545" y="440" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="u4D9dyohbDwP2OhtFbo0-3" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=1.002;entryY=0.144;entryDx=0;entryDy=0;entryPerimeter=0;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="aTO7ynV6Y0lAHxoPAONt-55" target="Jo9Ue6hLqtK935TEgpBK-19">
<mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="469" y="908" />
<mxPoint x="469" y="925" />
<mxPoint x="550" y="925" />
<mxPoint x="550" y="828" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="u4D9dyohbDwP2OhtFbo0-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.91;entryY=0.003;entryDx=0;entryDy=0;entryPerimeter=0;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="1-PaxUM0a53oYDua-TxR-3" target="1-PaxUM0a53oYDua-TxR-1">
<mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="487" y="1208" />
<mxPoint x="600" y="1208" />
<mxPoint x="600" y="1115" />
<mxPoint x="325" y="1115" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="u4D9dyohbDwP2OhtFbo0-5" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.25;entryDx=0;entryDy=0;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="mQTKtX7dmJeWt0bXEoog-1" target="mQTKtX7dmJeWt0bXEoog-1">
<mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="365" y="1370" />
<mxPoint x="365" y="1340" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="u4D9dyohbDwP2OhtFbo0-6" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.75;exitDx=0;exitDy=0;entryX=1;entryY=0.25;entryDx=0;entryDy=0;fontColor=#999999;strokeColor=#999999;" edge="1" parent="E9gbP4vXqY4O4zoDVCtS-33" source="Jo9Ue6hLqtK935TEgpBK-20" target="Jo9Ue6hLqtK935TEgpBK-20">
<mxGeometry relative="1" as="geometry">
<Array as="points">
<mxPoint x="365" y="1014" />
<mxPoint x="365" y="984" />
</Array>
</mxGeometry>
</mxCell> </mxCell>
<mxCell id="E9gbP4vXqY4O4zoDVCtS-37" value="rotary encoder  (control.cpp)" style="swimlane;labelBackgroundColor=none;fontColor=#000000;" parent="1" vertex="1"> <mxCell id="E9gbP4vXqY4O4zoDVCtS-37" value="rotary encoder  (control.cpp)" style="swimlane;labelBackgroundColor=none;fontColor=#000000;" parent="1" vertex="1">
<mxGeometry x="1245" y="1415" width="590" height="215" as="geometry" /> <mxGeometry x="1245" y="1415" width="590" height="215" as="geometry" />
@ -787,9 +811,10 @@
<mxGeometry x="-0.5375" relative="1" as="geometry"> <mxGeometry x="-0.5375" relative="1" as="geometry">
<mxPoint x="85" y="1120" as="targetPoint" /> <mxPoint x="85" y="1120" as="targetPoint" />
<Array as="points"> <Array as="points">
<mxPoint x="85" y="1470" /> <mxPoint x="85" y="1480" />
</Array> </Array>
<mxPoint x="1" as="offset" /> <mxPoint x="1" as="offset" />
<mxPoint x="231.5" y="1520" as="sourcePoint" />
</mxGeometry> </mxGeometry>
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-8" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.25;exitY=1;exitDx=0;exitDy=0;endArrow=oval;endFill=1;" parent="1" source="1-PaxUM0a53oYDua-TxR-1" edge="1"> <mxCell id="1-PaxUM0a53oYDua-TxR-8" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.25;exitY=1;exitDx=0;exitDy=0;endArrow=oval;endFill=1;" parent="1" source="1-PaxUM0a53oYDua-TxR-1" edge="1">
@ -801,7 +826,7 @@
</Array> </Array>
</mxGeometry> </mxGeometry>
</mxCell> </mxCell>
<mxCell id="1-PaxUM0a53oYDua-TxR-9" value="SW_RESET&lt;br&gt;SW_CUT&lt;br&gt;!SW_AUTO_CUT" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="1-PaxUM0a53oYDua-TxR-8" vertex="1" connectable="0"> <mxCell id="1-PaxUM0a53oYDua-TxR-9" value="any button&lt;br&gt;event" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="1-PaxUM0a53oYDua-TxR-8" vertex="1" connectable="0">
<mxGeometry x="-0.1235" y="2" relative="1" as="geometry"> <mxGeometry x="-0.1235" y="2" relative="1" as="geometry">
<mxPoint x="-22" y="-2" as="offset" /> <mxPoint x="-22" y="-2" as="offset" />
</mxGeometry> </mxGeometry>

Binary file not shown.

BIN
img/cable-length-cutter.jpg Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 546 KiB

3
img/panel-layout.svg Normal file

File diff suppressed because one or more lines are too long

After

Width:  |  Height:  |  Size: 348 KiB

View File

@ -1,7 +1,7 @@
idf_component_register( idf_component_register(
SRCS SRCS
"main.cpp" "main.cpp"
"config.cpp" "global.cpp"
"control.cpp" "control.cpp"
"buzzer.cpp" "buzzer.cpp"
"vfd.cpp" "vfd.cpp"
@ -11,6 +11,10 @@ idf_component_register(
"stepper.cpp" "stepper.cpp"
"guide-stepper.cpp" "guide-stepper.cpp"
"encoder.cpp" "encoder.cpp"
"shutdown.cpp"
INCLUDE_DIRS INCLUDE_DIRS
"." "."
) )
# colored build output (errors, warnings...)
idf_build_set_property(COMPILE_OPTIONS "-fdiagnostics-color=always" APPEND)

View File

@ -1,5 +1,5 @@
#include "buzzer.hpp" #include "buzzer.hpp"
#include "config.hpp" #include "config.h"
static const char *TAG_BUZZER = "buzzer"; static const char *TAG_BUZZER = "buzzer";

151
main/config.h Normal file
View File

@ -0,0 +1,151 @@
#pragma once
#include "esp_idf_version.h"
//note: global variables and objects were moved to global.hpp
//===================================
//===== define output gpio pins =====
//===================================
//4x stepper mosfet outputs for VFD
#define GPIO_VFD_FWD GPIO_NUM_4 //ST4
#define GPIO_VFD_REV GPIO_NUM_5 //mos2
#define GPIO_VFD_D0 GPIO_NUM_2 //ST2
#define GPIO_VFD_D1 GPIO_NUM_15 //ST1
//#define GPIO_VFD_D2 GPIO_NUM_15 //ST1 (D2 only used with 7.5kw vfd)
#define GPIO_MOS1 GPIO_NUM_18 //mos1 (free) 2022.02.28: pin used for stepper
#define GPIO_LAMP GPIO_NUM_0 //mos2 (5) 2022.02.28: lamp disabled, pin used for stepper
#define GPIO_RELAY GPIO_NUM_13
#define GPIO_BUZZER GPIO_NUM_12
//==================================
//==== define analog input pins ====
//==================================
#define GPIO_POTI GPIO_NUM_36
#define ADC_CHANNEL_POTI ADC1_CHANNEL_0
#define GPIO_4SW_TO_ANALOG GPIO_NUM_39
#define ADC_CHANNEL_4SW_TO_ANALOG ADC1_CHANNEL_3 //gpio 39
#define ADC_CHANNEL ADC_CHANNEL_0
//#define ADC_LOW_VOLTAGE_THRESHOLD 1000 //adc value where shut down is detected (store certain values before power loss)
#define GPIO_PIN GPIO_NUM_2
#define ADC_CHANNEL_SUPPLY_VOLTAGE ADC1_CHANNEL_7//gpio35 onboard supply voltage
//ADC1_CHANNEL_0 gpio36
//ADC1_CHANNEL_6 gpio_34
//ADC1_CHANNEL_3 gpio_39
//=====================================
//==== assign switches to objects =====
//=====================================
//see config.cpp for available evaluated switch objects
#define SW_START sw_gpio_26
#define SW_RESET sw_gpio_25
#define SW_CUTTER_POS sw_gpio_14
#define SW_SET sw_gpio_analog_0
#define SW_PRESET1 sw_gpio_analog_1
#define SW_PRESET2 sw_gpio_analog_2
#define SW_PRESET3 sw_gpio_analog_3
#define SW_CUT sw_gpio_33
#define SW_AUTO_CUT sw_gpio_32
//#define ? sw_gpio_34
//note: actual objects are created in global.cpp
//=============================
//======= configuration =======
//=============================
//--------------------------
//----- display config -----
//--------------------------
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(4, 0, 0)
#define HOST HSPI_HOST
#else
#define HOST SPI2_HOST
#endif
#define DISPLAY_PIN_NUM_MOSI GPIO_NUM_23
#define DISPLAY_PIN_NUM_CLK GPIO_NUM_22
#define DISPLAY_PIN_NUM_CS GPIO_NUM_27
#define DISPLAY_DELAY 2000
#define DISPLAY_BRIGHTNESS 8
//--------------------------
//----- encoder config -----
//--------------------------
#define ROT_ENC_A_GPIO GPIO_NUM_19
#define ROT_ENC_B_GPIO GPIO_NUM_21
#define ENABLE_HALF_STEPS false // Set to true to enable tracking of rotary encoder at half step resolution
#define FLIP_DIRECTION false // Set to true to reverse the clockwise/counterclockwise sense
//--------------------------
//----- stepper config -----
//--------------------------
//enable stepper test mode (dont start control and encoder task)
//#define STEPPER_TEST
//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 / spindle-slope)
#define STEPPER_SPEED_DEFAULT 25 //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 7 //steps/s decrement per cycle
//options affecting movement are currently defined in guide-stepper.cpp
//---------------------------
//------- cable guide -------
//---------------------------
// default axis coordinates the guide changes direction (winding width)
#define GUIDE_MIN_MM 0 // TODO add feature so guide stays at zero for some steps (negative MIN_MM?), currently seems appropriate for even winding
#define GUIDE_MAX_MM 90 // 95 still to long at max pos - actual reel is 110, but currently guide turned out to stay at max position for too long, due to cable running diagonal from guide to reel
// tolerance added to last stored position at previous shutdown.
// When calibrating at startup the stepper moves for that sum to get track of zero position (ensure crashes into hardware limit for at least some time)
#define AUTO_HOME_TRAVEL_ADD_TO_LAST_POS_MM 20
#define MAX_TOTAL_AXIS_TRAVEL_MM 103 // max possible travel distance, needed as fallback for auto-home
#define LAYER_THICKNESS_MM 5 // height of one cable layer on reel -> increase in radius every layer
#define D_CABLE 6 // determines winds per layer / guide speed
#define D_REEL 160 // start diameter of empty reel
// max winding width that can be set using potentiometer (SET+PRESET1 buttons)
#define MAX_SELECTABLE_WINDING_WIDTH_MM 100;
// max target length that can be selected using potentiometer (SET button)
#define MAX_SELECTABLE_LENGTH_POTI_MM 100000
// calculate new winding width each time target length changes, according to custom thresholds defined in guide-stepper.cpp
// if not defined, winding width is always GUIDE_MAX_MM even for short lengths
#define DYNAMIC_WINDING_WIDTH_ENABLED
//--------------------------
//------ calibration -------
//--------------------------
//enable mode encoder test for calibration (determine ENCODER_STEPS_PER_METER)
//if defined, displays always show length and steps instead of the normal messages
//#define ENCODER_TEST
//TODO: add way to calibrate without flashing -> enter calibration mode with certain button sequence, enter steps-per-meter with poti, store in nvs
//steps per meter
//this value is determined experimentally while ENCODER_TEST is enabled
//#define ENCODER_STEPS_PER_METER 2127 //until 2024.03.13 roll-v3-gummi-86.6mm - d=89.8mm
#define ENCODER_STEPS_PER_METER 2118 //2024.03.13 roll-v3-gummi measured 86.5mm
//millimeters added to target length
//to ensure that length does not fall short when spool slightly rotates back after stop
#define TARGET_LENGTH_OFFSET 0
//millimeters lengthNow can be below lengthTarget to still stay in target_reached state
#define TARGET_REACHED_TOLERANCE 5

View File

@ -1,138 +0,0 @@
#pragma once
extern "C" {
#include "driver/adc.h"
}
#include "gpio_evaluateSwitch.hpp"
#include "buzzer.hpp"
#include "switchesAnalog.hpp"
//===================================
//===== define output gpio pins =====
//===================================
//4x stepper mosfet outputs for VFD
#define GPIO_VFD_FWD GPIO_NUM_4 //ST4
#define GPIO_VFD_REV GPIO_NUM_5 //mos2
#define GPIO_VFD_D0 GPIO_NUM_2 //ST2
#define GPIO_VFD_D1 GPIO_NUM_15 //ST1
//#define GPIO_VFD_D2 GPIO_NUM_15 //ST1 (D2 only used with 7.5kw vfd)
#define GPIO_MOS1 GPIO_NUM_18 //mos1 (free) 2022.02.28: pin used for stepper
#define GPIO_LAMP GPIO_NUM_0 //mos2 (5) 2022.02.28: lamp disabled, pin used for stepper
#define GPIO_RELAY GPIO_NUM_13
#define GPIO_BUZZER GPIO_NUM_12
//==================================
//==== define analog input pins ====
//==================================
#define GPIO_POTI GPIO_NUM_36
#define ADC_CHANNEL_POTI ADC1_CHANNEL_0
#define GPIO_4SW_TO_ANALOG GPIO_NUM_39
#define ADC_CHANNEL_4SW_TO_ANALOG ADC1_CHANNEL_3 //gpio 39
//ADC1_CHANNEL_0 gpio36
//ADC1_CHANNEL_6 gpio_34
//ADC1_CHANNEL_3 gpio_39
//=====================================
//==== assign switches to objects =====
//=====================================
//see config.cpp for available evaluated switch objects
#define SW_START sw_gpio_26
#define SW_RESET sw_gpio_25
#define SW_CUTTER_POS sw_gpio_14
#define SW_SET sw_gpio_analog_0
#define SW_PRESET1 sw_gpio_analog_1
#define SW_PRESET2 sw_gpio_analog_2
#define SW_PRESET3 sw_gpio_analog_3
#define SW_CUT sw_gpio_33
#define SW_AUTO_CUT sw_gpio_32
//unused but already available evaluated inputs
//#define ? sw_gpio_34
//=============================
//======= configuration =======
//=============================
//--------------------------
//----- display config -----
//--------------------------
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(4, 0, 0)
#define HOST HSPI_HOST
#else
#define HOST SPI2_HOST
#endif
#define DISPLAY_PIN_NUM_MOSI GPIO_NUM_23
#define DISPLAY_PIN_NUM_CLK GPIO_NUM_22
#define DISPLAY_PIN_NUM_CS GPIO_NUM_27
#define DISPLAY_DELAY 2000
//--------------------------
//----- encoder config -----
//--------------------------
#define ROT_ENC_A_GPIO GPIO_NUM_19
#define ROT_ENC_B_GPIO GPIO_NUM_21
#define ENABLE_HALF_STEPS false // Set to true to enable tracking of rotary encoder at half step resolution
#define FLIP_DIRECTION false // Set to true to reverse the clockwise/counterclockwise sense
//--------------------------
//----- stepper config -----
//--------------------------
//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
//--------------------------
//------ calibration -------
//--------------------------
//enable mode encoder test for calibration
//if defined, displays always show length and steps instead of the normal messages
//#define ENCODER_TEST
//steps per meter
#define ENCODER_STEPS_PER_METER 2127 //roll-v3-gummi-86.6mm - d=89.8mm
//millimetres added to target length
//to ensure that length does not fall short when spool slightly rotates back after stop
#define TARGET_LENGTH_OFFSET 0
//millimetres lengthNow can be below lengthTarget to still stay in target_reached state
#define TARGET_REACHED_TOLERANCE 5
//============================
//===== global variables =====
//============================
//create global evaluated switch objects
//--- switches on digital gpio pins ---
//extern gpio_evaluatedSwitch sw_gpio_39;
extern gpio_evaluatedSwitch sw_gpio_34;
extern gpio_evaluatedSwitch sw_gpio_32;
extern gpio_evaluatedSwitch sw_gpio_33;
extern gpio_evaluatedSwitch sw_gpio_25;
extern gpio_evaluatedSwitch sw_gpio_26;
extern gpio_evaluatedSwitch sw_gpio_14;
//--- switches connected to 4-sw-to-analog stripboard ---
extern gpio_evaluatedSwitch sw_gpio_analog_0;
extern gpio_evaluatedSwitch sw_gpio_analog_1;
extern gpio_evaluatedSwitch sw_gpio_analog_2;
extern gpio_evaluatedSwitch sw_gpio_analog_3;
//create global buzzer object
extern buzzer_t buzzer;

View File

@ -1,20 +1,47 @@
#include "control.hpp" extern "C"
{
#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_idf_version.h>
#include "freertos/queue.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/adc.h"
#include "max7219.h"
}
#include <cmath>
#include "config.h"
#include "gpio_evaluateSwitch.hpp"
#include "gpio_adc.hpp"
#include "buzzer.hpp"
#include "vfd.hpp"
#include "display.hpp"
#include "cutter.hpp"
#include "encoder.hpp" #include "encoder.hpp"
#include "guide-stepper.hpp"
#include "global.hpp"
#include "control.hpp"
//==================== //-----------------------------------------
//==== variables ===== //--------------- variables ---------------
//==================== //-----------------------------------------
static const char *TAG = "control"; //tag for logging static const char *TAG = "control"; //tag for logging
//control
const char* systemStateStr[7] = {"COUNTING", "WINDING_START", "WINDING", "TARGET_REACHED", "AUTO_CUT_WAITING", "CUTTING", "MANUAL"}; const char* systemStateStr[7] = {"COUNTING", "WINDING_START", "WINDING", "TARGET_REACHED", "AUTO_CUT_WAITING", "CUTTING", "MANUAL"};
systemState_t controlState = systemState_t::COUNTING; systemState_t controlState = systemState_t::COUNTING;
static uint32_t timestamp_changedState = 0; static uint32_t timestamp_lastStateChange = 0;
//display
static char buf_disp1[10];// 8 digits + decimal point + \0 static char buf_disp1[10];// 8 digits + decimal point + \0
static char buf_disp2[10];// 8 digits + decimal point + \0 static char buf_disp2[10];// 8 digits + decimal point + \0
static char buf_tmp[15]; static char buf_tmp[15];
//track length
static int lengthNow = 0; //length measured in mm static int lengthNow = 0; //length measured in mm
static int lengthTarget = 5000; //default target length in mm static int lengthTarget = 5000; //default target length in mm
static int lengthRemaining = 0; //(target - now) length needed for reaching the target static int lengthRemaining = 0; //(target - now) length needed for reaching the target
@ -27,8 +54,19 @@ static uint32_t timestamp_cut_lastBeep = 0;
static uint32_t autoCut_delayMs = 2500; //TODO add this to config static uint32_t autoCut_delayMs = 2500; //TODO add this to config
static bool autoCutEnabled = false; //store state of toggle switch (no hotswitch) static bool autoCutEnabled = false; //store state of toggle switch (no hotswitch)
//user interface
static uint32_t timestamp_lastWidthSelect = 0;
//ignore new set events for that time after last value set using poti
#define DEAD_TIME_POTI_SET_VALUE 1000
//-----------------------------------------
//--------------- functions ---------------
//-----------------------------------------
//========================
//===== change State ===== //===== change State =====
//========================
//function for changing the controlState with log output //function for changing the controlState with log output
void changeState (systemState_t stateNew) { void changeState (systemState_t stateNew) {
//only proceed when state actually changed //only proceed when state actually changed
@ -40,18 +78,21 @@ void changeState (systemState_t stateNew) {
//change state //change state
controlState = stateNew; controlState = stateNew;
//update timestamp //update timestamp
timestamp_changedState = esp_log_timestamp(); timestamp_lastStateChange = esp_log_timestamp();
} }
//=================================
//===== handle Stop Condition ===== //===== handle Stop Condition =====
//function that checks whether start button is released or target is reached (used in multiple states) //=================================
//returns true when stopped, false when no action //function that checks whether start button is released or target is reached
//and takes according action if so (used in multiple states)
//returns true when stop condition was met, false when no action required
bool handleStopCondition(handledDisplay * displayTop, handledDisplay * displayBot){ bool handleStopCondition(handledDisplay * displayTop, handledDisplay * displayBot){
//--- stop conditions --- //--- stop conditions ---
//stop conditions that are checked in any mode //stop conditions that are checked in any mode
//target reached //target reached -> reached state, stop motor, display message
if (lengthRemaining <= 0 ) { if (lengthRemaining <= 0 ) {
changeState(systemState_t::TARGET_REACHED); changeState(systemState_t::TARGET_REACHED);
vfd_setState(false); vfd_setState(false);
@ -60,7 +101,7 @@ bool handleStopCondition(handledDisplay * displayTop, handledDisplay * displayBo
buzzer.beep(2, 100, 100); buzzer.beep(2, 100, 100);
return true; return true;
} }
//start button released //start button released -> idle state, stop motor, display message
else if (SW_START.state == false) { else if (SW_START.state == false) {
changeState(systemState_t::COUNTING); changeState(systemState_t::COUNTING);
vfd_setState(false); vfd_setState(false);
@ -74,8 +115,11 @@ bool handleStopCondition(handledDisplay * displayTop, handledDisplay * displayBo
} }
//===================================
//===== set dynamic speed level ===== //===== set dynamic speed level =====
//function that sets the vfd speed level dynamicly depending on the remaining distance //===================================
//function that sets the vfd speed level dynamically depending on the remaining distance
//closer to target -> slower //closer to target -> slower
void setDynSpeedLvl(uint8_t lvlMax = 3){ void setDynSpeedLvl(uint8_t lvlMax = 3){
uint8_t lvl; uint8_t lvl;
@ -103,31 +147,33 @@ void setDynSpeedLvl(uint8_t lvlMax = 3){
//======================== //========================
//===== control task ===== //===== control task =====
//======================== //========================
//task that controls the entire machine
void task_control(void *pvParameter) void task_control(void *pvParameter)
{ {
//-- initialize display --
//initialize display
max7219_t two7SegDisplays = display_init(); max7219_t two7SegDisplays = display_init();
//create two separate handled display instances //create two separate custom handled display instances
handledDisplay displayTop(two7SegDisplays, 0); handledDisplay displayTop(two7SegDisplays, 0);
handledDisplay displayBot(two7SegDisplays, 8); handledDisplay displayBot(two7SegDisplays, 8);
//--- display welcome msg --- //-- display welcome msg --
//display welcome message on two 7 segment displays //display welcome message on two 7 segment displays
//currently show name and date and scrolling 'hello' //currently show name and date and scrolling 'hello'
display_ShowWelcomeMsg(two7SegDisplays); display_ShowWelcomeMsg(two7SegDisplays);
//-- set initial winding width for default length --
guide_setWindingWidth(guide_targetLength2WindingWidth(lengthTarget));
//================ // ##############################
//===== loop ===== // ######## control loop ########
//================ // ##############################
// repeatedly handle the machine
while(1){ while(1){
vTaskDelay(10 / portTICK_PERIOD_MS); vTaskDelay(10 / portTICK_PERIOD_MS);
//-----------------------------
//------ handle switches ------ //------ handle switches ------
//----------------------------- //run handle functions for all switches used here
//run handle functions for all switches
SW_START.handle(); SW_START.handle();
SW_RESET.handle(); SW_RESET.handle();
SW_SET.handle(); SW_SET.handle();
@ -138,27 +184,23 @@ void task_control(void *pvParameter)
SW_AUTO_CUT.handle(); SW_AUTO_CUT.handle();
//---------------------------
//------ handle cutter ------ //------ handle cutter ------
//--------------------------- //TODO: separate task for cutter?
cutter_handle(); cutter_handle();
//----------------------------
//------ rotary encoder ------ //------ rotary encoder ------
//---------------------------- //get current length since last reset
//--- get current length since last reset ---
lengthNow = encoder_getLenMm(); lengthNow = encoder_getLenMm();
//---------------------------
//--------- buttons --------- //--------- buttons ---------
//--------------------------- //#### RESET switch ####
//--- RESET switch ---
if (SW_RESET.risingEdge) { if (SW_RESET.risingEdge) {
//dont reset when press used for stopping pending auto-cut //dont reset when press used for stopping pending auto-cut
if (controlState != systemState_t::AUTO_CUT_WAITING) { if (controlState != systemState_t::AUTO_CUT_WAITING) {
encoder_reset(); guide_moveToZero(); //move axis guiding the cable to start position
encoder_reset(); //reset length measurement
lengthNow = 0; lengthNow = 0;
buzzer.beep(1, 700, 100); buzzer.beep(1, 700, 100);
displayTop.blink(2, 100, 100, "1ST "); displayTop.blink(2, 100, 100, "1ST ");
@ -167,7 +209,7 @@ void task_control(void *pvParameter)
} }
} }
//--- CUT switch --- //### CUT switch ####
//start cut cycle immediately //start cut cycle immediately
if (SW_CUT.risingEdge) { if (SW_CUT.risingEdge) {
//stop cutter if already running //stop cutter if already running
@ -190,7 +232,7 @@ void task_control(void *pvParameter)
} }
} }
//--- AUTO_CUT toggle sw --- //#### AUTO_CUT toggle sw ####
//beep at change //beep at change
if (SW_AUTO_CUT.risingEdge) { if (SW_AUTO_CUT.risingEdge) {
buzzer.beep(2, 100, 50); buzzer.beep(2, 100, 50);
@ -209,7 +251,7 @@ void task_control(void *pvParameter)
autoCutEnabled = false; autoCutEnabled = false;
} }
//--- manual mode --- //#### manual mode ####
//switch to manual motor control (2 buttons + poti) //switch to manual motor control (2 buttons + poti)
if ( SW_PRESET2.state && (SW_PRESET1.state || SW_PRESET3.state) && controlState != systemState_t::MANUAL ) { if ( SW_PRESET2.state && (SW_PRESET1.state || SW_PRESET3.state) && controlState != systemState_t::MANUAL ) {
//enable manual control //enable manual control
@ -217,13 +259,44 @@ void task_control(void *pvParameter)
buzzer.beep(3, 100, 60); buzzer.beep(3, 100, 60);
} }
//--- set custom target length --- //##### SET switch + Potentiometer #####
//set target length to poti position when SET switch is pressed //## set winding-width (SET+PRESET1+POTI) ##
if (SW_SET.state == true) { // set winding width (axis travel) with poti position
// when SET and PRESET1 button are pressed
if (SW_SET.state == true && SW_PRESET1.state == true) {
timestamp_lastWidthSelect = esp_log_timestamp();
//read adc //read adc
potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095 potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095
//scale to target length range //scale to target length range
int lengthTargetNew = (float)potiRead / 4095 * 50000; uint8_t windingWidthNew = (float)potiRead / 4095 * MAX_SELECTABLE_WINDING_WIDTH_MM;
//apply hysteresis and round to whole meters //TODO optimize this
if (windingWidthNew % 5 < 2) { //round down if remainder less than 2mm
ESP_LOGD(TAG, "Poti input = %d -> rounding down", windingWidthNew);
windingWidthNew = (windingWidthNew/5 ) * 5; //round down
} else if (windingWidthNew % 5 > 4 ) { //round up if remainder more than 4mm
ESP_LOGD(TAG, "Poti input = %d -> rounding up", windingWidthNew);
windingWidthNew = (windingWidthNew/5 + 1) * 5; //round up
} else {
ESP_LOGD(TAG, "Poti input = %d -> hysteresis", windingWidthNew);
windingWidthNew = guide_getWindingWidth();
}
//update target width and beep when effectively changed
if (windingWidthNew != guide_getWindingWidth()) {
//TODO update at button release only?
guide_setWindingWidth(windingWidthNew);
ESP_LOGW(TAG, "Changed winding width to %d mm", windingWidthNew);
buzzer.beep(1, 30, 10);
}
}
//## set target length (SET+POTI) ##
//set target length to poti position when only SET button is pressed and certain dead time passed after last setWindingWidth (SET and PRESET1 button) to prevent set target at release
// FIXME: when going to edit the winding width (SET+PRESET1) sometimes the target-length also updates when initially pressing SET -> update only at actual poti change (works sometimes)
else if (SW_SET.state == true && (esp_log_timestamp() - timestamp_lastWidthSelect > DEAD_TIME_POTI_SET_VALUE)) {
//read adc
potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095
//scale to target length range
int lengthTargetNew = (float)potiRead / 4095 * MAX_SELECTABLE_LENGTH_POTI_MM;
//apply hysteresis and round to whole meters //TODO optimize this //apply hysteresis and round to whole meters //TODO optimize this
if (lengthTargetNew % 1000 < 200) { //round down if less than .2 meter if (lengthTargetNew % 1000 < 200) { //round down if less than .2 meter
ESP_LOGD(TAG, "Poti input = %d -> rounding down", lengthTargetNew); ESP_LOGD(TAG, "Poti input = %d -> rounding down", lengthTargetNew);
@ -239,6 +312,7 @@ void task_control(void *pvParameter)
if (lengthTargetNew != lengthTarget) { if (lengthTargetNew != lengthTarget) {
//TODO update lengthTarget only at button release? //TODO update lengthTarget only at button release?
lengthTarget = lengthTargetNew; lengthTarget = lengthTargetNew;
guide_setWindingWidth(guide_targetLength2WindingWidth(lengthTarget));
ESP_LOGI(TAG, "Changed target length to %d mm", lengthTarget); ESP_LOGI(TAG, "Changed target length to %d mm", lengthTarget);
buzzer.beep(1, 25, 10); buzzer.beep(1, 25, 10);
} }
@ -253,36 +327,40 @@ void task_control(void *pvParameter)
} }
//--- target length presets --- //##### target length preset buttons #####
if (controlState != systemState_t::MANUAL) { //dont apply preset length while controlling motor with preset buttons if (controlState != systemState_t::MANUAL && SW_SET.state == false) { //dont apply preset length while controlling motor with preset buttons
if (SW_PRESET1.risingEdge) { if (SW_PRESET1.risingEdge) {
lengthTarget = 5000; lengthTarget = 5000;
guide_setWindingWidth(guide_targetLength2WindingWidth(lengthTarget));
buzzer.beep(lengthTarget/1000, 25, 30); buzzer.beep(lengthTarget/1000, 25, 30);
displayBot.blink(2, 100, 100, "S0LL "); displayBot.blink(2, 100, 100, "S0LL ");
} }
else if (SW_PRESET2.risingEdge) { else if (SW_PRESET2.risingEdge) {
lengthTarget = 10000; lengthTarget = 10000;
guide_setWindingWidth(guide_targetLength2WindingWidth(lengthTarget));
buzzer.beep(lengthTarget/1000, 25, 30); buzzer.beep(lengthTarget/1000, 25, 30);
displayBot.blink(2, 100, 100, "S0LL "); displayBot.blink(2, 100, 100, "S0LL ");
} }
else if (SW_PRESET3.risingEdge) { else if (SW_PRESET3.risingEdge) {
lengthTarget = 15000; lengthTarget = 15000;
guide_setWindingWidth(guide_targetLength2WindingWidth(lengthTarget));
buzzer.beep(lengthTarget/1000, 25, 30); buzzer.beep(lengthTarget/1000, 25, 30);
displayBot.blink(2, 100, 100, "S0LL "); displayBot.blink(2, 100, 100, "S0LL ");
} }
} }
//--------------------------- //---------------------------
//--------- control --------- //--------- control ---------
//--------------------------- //---------------------------
//calculate length difference //statemachine handling the sequential winding process
//calculate current length difference
lengthRemaining = lengthTarget - lengthNow + TARGET_LENGTH_OFFSET; lengthRemaining = lengthTarget - lengthNow + TARGET_LENGTH_OFFSET;
//--- statemachine --- //--- statemachine ---
switch (controlState) { switch (controlState) {
case systemState_t::COUNTING: //no motor action case systemState_t::COUNTING: //no motor action, just show current length on display
vfd_setState(false); vfd_setState(false);
//TODO check stop condition before starting - prevents motor from starting 2 cycles when already at target //TODO check stop condition before starting - prevents motor from starting 2 cycles when already at target
//--- start winding to length --- //--- start winding to length ---
@ -298,6 +376,7 @@ void task_control(void *pvParameter)
case systemState_t::WINDING_START: //wind slow for certain time case systemState_t::WINDING_START: //wind slow for certain time
//set vfd speed depending on remaining distance //set vfd speed depending on remaining distance
setDynSpeedLvl(1); //limit to speed lvl 1 (force slow start) setDynSpeedLvl(1); //limit to speed lvl 1 (force slow start)
//switch to WINDING state (full speed) when 3s have passed
if (esp_log_timestamp() - timestamp_motorStarted > 3000) { if (esp_log_timestamp() - timestamp_motorStarted > 3000) {
changeState(systemState_t::WINDING); changeState(systemState_t::WINDING);
} }
@ -307,23 +386,23 @@ void task_control(void *pvParameter)
case systemState_t::WINDING: //wind fast, slow down when close case systemState_t::WINDING: //wind fast, slow down when close
//set vfd speed depending on remaining distance //set vfd speed depending on remaining distance
setDynSpeedLvl(); //slow down when close to target setDynSpeedLvl(); //set motor speed, slow down when close to target
handleStopCondition(&displayTop, &displayBot); //stops if button released or target reached handleStopCondition(&displayTop, &displayBot); //stops if button released or target reached
//TODO: cancel when there is no cable movement anymore e.g. empty / timeout? //TODO: cancel when there is no cable movement anymore e.g. empty / timeout?
break; break;
case systemState_t::TARGET_REACHED: case systemState_t::TARGET_REACHED: //prevent further motor rotation and start auto-cut
vfd_setState(false); vfd_setState(false);
//switch to counting state when no longer at or above target length //switch to counting state when no longer at or above target length
if ( lengthNow < lengthTarget - TARGET_REACHED_TOLERANCE ) { if ( lengthNow < lengthTarget - TARGET_REACHED_TOLERANCE ) {
changeState(systemState_t::COUNTING); changeState(systemState_t::COUNTING);
} }
//switch initiate countdown to auto-cut //initiate countdown to auto-cut if enabled
else if ( (autoCutEnabled) else if ( (autoCutEnabled)
&& (esp_log_timestamp() - timestamp_changedState > 300) ) { //wait for dislay msg "reached" to finish && (esp_log_timestamp() - timestamp_lastStateChange > 300) ) { //wait for dislay msg "reached" to finish
changeState(systemState_t::AUTO_CUT_WAITING); changeState(systemState_t::AUTO_CUT_WAITING);
} }
//show msg when trying to start, but target is reached //show msg when trying to start, but target is already reached (-> reset button has to be pressed)
if (SW_START.risingEdge) { if (SW_START.risingEdge) {
buzzer.beep(2, 50, 30); buzzer.beep(2, 50, 30);
displayTop.blink(2, 600, 500, " S0LL "); displayTop.blink(2, 600, 500, " S0LL ");
@ -331,9 +410,8 @@ void task_control(void *pvParameter)
} }
break; break;
case systemState_t::AUTO_CUT_WAITING: case systemState_t::AUTO_CUT_WAITING: //handle delayed start of cut
//handle delayed start of cut cut_msRemaining = autoCut_delayMs - (esp_log_timestamp() - timestamp_lastStateChange);
cut_msRemaining = autoCut_delayMs - (esp_log_timestamp() - timestamp_changedState);
//- countdown stop conditions - //- countdown stop conditions -
//stop with any button //stop with any button
if (!autoCutEnabled if (!autoCutEnabled
@ -357,13 +435,14 @@ void task_control(void *pvParameter)
} }
break; break;
case systemState_t::CUTTING: case systemState_t::CUTTING: //prevent any action while cutter is active
//exit when finished cutting //exit when finished cutting
if (cutter_isRunning() == false) { if (cutter_isRunning() == false) {
//TODO stop if start buttons released? //TODO stop if start buttons released?
changeState(systemState_t::COUNTING); changeState(systemState_t::COUNTING);
//TODO reset automatically or wait for manual reset? //TODO reset automatically or wait for manual reset?
encoder_reset(); guide_moveToZero(); //move axis guiding the cable to start position
encoder_reset(); //reset length measurement
lengthNow = 0; lengthNow = 0;
buzzer.beep(1, 700, 100); buzzer.beep(1, 700, 100);
} }
@ -400,30 +479,34 @@ void task_control(void *pvParameter)
} }
#ifdef ENCODER_TEST
//-------------------------- //--------------------------
//------ encoder test ------ //------ encoder test ------
//-------------------------- //--------------------------
#ifdef ENCODER_TEST //mode for calibrating the cable length measurement (determine ENCODER_STEPS_PER_METER in config.h)
//run display handle functions //run display handle functions
displayTop.handle(); displayTop.handle();
displayBot.handle(); displayBot.handle();
//-- show encoder steps on display1 --- //-- show encoder steps on display1 ---
sprintf(buf_disp1, "EN %05d", encoder_getSteps); //count sprintf(buf_disp1, "EN %05d", encoder_getSteps()); //count
displayTop.showString(buf_disp1); displayTop.showString(buf_disp1);
//--- show converted distance on display2 --- //--- show converted distance on display2 ---
sprintf(buf_disp2, "Met %5.3f", (float)lengthNow/1000); //m sprintf(buf_disp2, "Met %5.3f", (float)lengthNow/1000); //m
displayBot.showString(buf_disp2); displayBot.showString(buf_disp2);
//--- beep every 1m --- //--- beep every 0.5m ---
//note: only works precicely in forward/positive direction //note: only works precisely in forward/positive direction, in reverse it it beeps by tolerance too early
if (lengthNow % 1000 < 50) { //with tolerance in case of missed exact value static int lengthBeeped = 0;
if (fabs(lengthNow - lengthBeeped) >= 900) { //dont beep multiple times at same meter if (lengthNow % 500 < 50) { //with tolerance in case of missed exact value
//TODO: add case for reverse direction. currently beeps 0.1 too early if (fabs(lengthNow - lengthBeeped) >= 400) { //dont beep multiple times at same distance
buzzer.beep(1, 400, 100 ); //TODO: add case for reverse direction. currently beeps 50mm too early
if (lengthNow % 1000 < 50) // 1m beep
buzzer.beep(1, 400, 100);
else // 0.5m beep
buzzer.beep(1, 200, 100);
lengthBeeped = lengthNow; lengthBeeped = lengthNow;
} }
} }
#else #else //not in encoder calibration mode
//-------------------------- //--------------------------
//-------- display1 -------- //-------- display1 --------
@ -434,6 +517,10 @@ void task_control(void *pvParameter)
if (controlState == systemState_t::AUTO_CUT_WAITING) { if (controlState == systemState_t::AUTO_CUT_WAITING) {
displayTop.blinkStrings(" CUT 1N ", " ", 70, 30); displayTop.blinkStrings(" CUT 1N ", " ", 70, 30);
} }
//setting winding width: blink info message
else if (SW_SET.state && SW_PRESET1.state){
displayTop.blinkStrings("SET WIND", " WIDTH ", 900, 900);
}
//otherwise show current position //otherwise show current position
else { else {
sprintf(buf_tmp, "1ST %5.4f", (float)lengthNow/1000); sprintf(buf_tmp, "1ST %5.4f", (float)lengthNow/1000);
@ -443,23 +530,13 @@ void task_control(void *pvParameter)
displayTop.showString(buf_disp1); displayTop.showString(buf_disp1);
} }
//-------------------------- //--------------------------
//-------- display2 -------- //-------- display2 --------
//-------------------------- //--------------------------
//run handle function //run handle function
displayBot.handle(); displayBot.handle();
//setting target length: blink target length
if (SW_SET.state == true) {
sprintf(buf_tmp, "S0LL%5.3f", (float)lengthTarget/1000);
displayBot.blinkStrings(buf_tmp, "S0LL ", 300, 100);
}
//manual state: blink "manual"
else if (controlState == systemState_t::MANUAL) {
displayBot.blinkStrings(" MANUAL ", buf_disp2, 400, 800);
}
//notify that cutter is active //notify that cutter is active
else if (cutter_isRunning()) { if (cutter_isRunning()) {
displayBot.blinkStrings("CUTTING]", "CUTTING[", 100, 100); displayBot.blinkStrings("CUTTING]", "CUTTING[", 100, 100);
} }
//show ms countdown to cut when pending //show ms countdown to cut when pending
@ -468,6 +545,20 @@ void task_control(void *pvParameter)
//displayBot.showString(buf_disp2); //TODO:blink "erreicht" overrides this. for now using blink as workaround //displayBot.showString(buf_disp2); //TODO:blink "erreicht" overrides this. for now using blink as workaround
displayBot.blinkStrings(buf_disp2, buf_disp2, 100, 100); displayBot.blinkStrings(buf_disp2, buf_disp2, 100, 100);
} }
//manual state: blink "manual"
else if (controlState == systemState_t::MANUAL) {
displayBot.blinkStrings(" MANUAL ", buf_disp2, 400, 800);
}
//setting winding width: blink currently set windingWidth
else if (SW_SET.state && SW_PRESET1.state){
sprintf(buf_tmp, " %03d mm", guide_getWindingWidth());
displayBot.blinkStrings(buf_tmp, " ", 300, 100);
}
//setting target length: blink target length
else if (SW_SET.state == true) {
sprintf(buf_tmp, "S0LL%5.3f", (float)lengthTarget/1000);
displayBot.blinkStrings(buf_tmp, "S0LL ", 300, 100);
}
//otherwise show target length //otherwise show target length
else { else {
//sprintf(buf_disp2, "%06.1f cm", (float)lengthTarget/10); //cm //sprintf(buf_disp2, "%06.1f cm", (float)lengthTarget/10); //cm
@ -476,6 +567,7 @@ void task_control(void *pvParameter)
displayBot.showString(buf_tmp); displayBot.showString(buf_tmp);
} }
#endif // end else ifdef ENCODER_TEST
//---------------------------- //----------------------------
//------- control lamp ------- //------- control lamp -------
@ -492,10 +584,6 @@ void task_control(void *pvParameter)
gpio_set_level(GPIO_LAMP, 0); gpio_set_level(GPIO_LAMP, 0);
} }
} //end while(1)
} //end task_control
#endif
}
}

View File

@ -1,34 +1,12 @@
#pragma once #pragma once
extern "C"
{
#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_idf_version.h>
#include "freertos/queue.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/adc.h"
#include "max7219.h"
}
#include <cmath>
#include "config.hpp"
#include "gpio_evaluateSwitch.hpp"
#include "gpio_adc.hpp"
#include "buzzer.hpp"
#include "vfd.hpp"
#include "display.hpp"
#include "cutter.hpp"
//enum describing the state of the system //enum describing the state of the system
enum class systemState_t {COUNTING, WINDING_START, WINDING, TARGET_REACHED, AUTO_CUT_WAITING, CUTTING, MANUAL}; enum class systemState_t {COUNTING, WINDING_START, WINDING, TARGET_REACHED, AUTO_CUT_WAITING, CUTTING, MANUAL};
//array with enum as strings for logging states //array with enum as strings for logging states
extern const char* systemStateStr[7]; extern const char* systemStateStr[7];
//task that controls the entire machine (has to be created as task in main function) //task that controls the entire machine (has to be created as task in main function)
void task_control(void *pvParameter); void task_control(void *pvParameter);

View File

@ -1,4 +1,6 @@
#include "cutter.hpp" #include "cutter.hpp"
#include "config.h"
#include "global.hpp"
const char* cutter_stateStr[5] = {"IDLE", "START", "CUTTING", "CANCELED", "TIMEOUT"}; //define strings for logging the state const char* cutter_stateStr[5] = {"IDLE", "START", "CUTTING", "CANCELED", "TIMEOUT"}; //define strings for logging the state
@ -39,7 +41,6 @@ void cutter_stop(){
if(cutter_state != cutter_state_t::IDLE){ if(cutter_state != cutter_state_t::IDLE){
setState(cutter_state_t::CANCELED); setState(cutter_state_t::CANCELED);
} }
//starts motor on state change
} }
@ -131,7 +132,6 @@ void cutter_handle(){
//SW_CUTTER_POS.minOnMs = 10; //SW_CUTTER_POS.minOnMs = 10;
//SW_CUTTER_POS.minOffMs = 10; //SW_CUTTER_POS.minOffMs = 10;
switch(cutter_state){ switch(cutter_state){
case cutter_state_t::IDLE: case cutter_state_t::IDLE:
case cutter_state_t::TIMEOUT: case cutter_state_t::TIMEOUT:
@ -142,14 +142,13 @@ void cutter_handle(){
case cutter_state_t::START: case cutter_state_t::START:
//--- moved away from idle position --- //--- moved away from idle position ---
//if (gpio_get_level(GPIO_CUTTER_POS_SW) == 0){ //contact closed //if (gpio_get_level(GPIO_CUTTER_POS_SW) == 0){ //contact closed
if (SW_CUTTER_POS.state == true) { //contact closed -> not at idle pos if (SW_CUTTER_POS.state == true) { //contact closed -> not at idle pos anymore
setState(cutter_state_t::CUTTING); setState(cutter_state_t::CUTTING);
} }
//--- timeout --- //--- timeout ---
else { else {
checkTimeout(); checkTimeout();
} }
break; break;
case cutter_state_t::CUTTING: case cutter_state_t::CUTTING:

View File

@ -1,5 +1,6 @@
#include "display.hpp" #include "display.hpp"
#include "config.h"
//=== variables === //=== variables ===
static const char *TAG = "display"; //tag for logging static const char *TAG = "display"; //tag for logging
@ -34,7 +35,7 @@ max7219_t display_init(){
ESP_ERROR_CHECK(max7219_init_desc(&dev, HOST, MAX7219_MAX_CLOCK_SPEED_HZ, DISPLAY_PIN_NUM_CS)); ESP_ERROR_CHECK(max7219_init_desc(&dev, HOST, MAX7219_MAX_CLOCK_SPEED_HZ, DISPLAY_PIN_NUM_CS));
ESP_ERROR_CHECK(max7219_init(&dev)); ESP_ERROR_CHECK(max7219_init(&dev));
//0...15 //0...15
ESP_ERROR_CHECK(max7219_set_brightness(&dev, 8)); //TODO add this to config ESP_ERROR_CHECK(max7219_set_brightness(&dev, DISPLAY_BRIGHTNESS));
return dev; return dev;
//display = dev; //display = dev;
ESP_LOGI(TAG, "initializing display - done"); ESP_LOGI(TAG, "initializing display - done");
@ -50,7 +51,7 @@ void display_ShowWelcomeMsg(max7219_t dev){
//show name and date //show name and date
ESP_LOGI(TAG, "showing startup message..."); ESP_LOGI(TAG, "showing startup message...");
max7219_clear(&dev); max7219_clear(&dev);
max7219_draw_text_7seg(&dev, 0, "CUTTER 29.09.2022"); max7219_draw_text_7seg(&dev, 0, "CUTTER 15.03.2024");
// 1234567812 34 5678 // 1234567812 34 5678
vTaskDelay(pdMS_TO_TICKS(700)); vTaskDelay(pdMS_TO_TICKS(700));
//scroll "hello" over 2 displays //scroll "hello" over 2 displays
@ -64,7 +65,7 @@ void display_ShowWelcomeMsg(max7219_t dev){
//noticed rare bug that one display does not initialize / is not configured correctly after start //noticed rare bug that one display does not initialize / is not configured correctly after start
//initialize display again after the welcome message in case it did not work the first time //initialize display again after the welcome message in case it did not work the first time
ESP_ERROR_CHECK(max7219_init(&dev)); ESP_ERROR_CHECK(max7219_init(&dev));
ESP_ERROR_CHECK(max7219_set_brightness(&dev, 8)); //TODO add this to config ESP_ERROR_CHECK(max7219_set_brightness(&dev, DISPLAY_BRIGHTNESS));
} }
@ -83,9 +84,9 @@ handledDisplay::handledDisplay(max7219_t displayDevice, uint8_t posStart_f) {
//-------------------------------- //================================
//---------- showString ---------- //========== showString ==========
//-------------------------------- //================================
//function that displays a given string on the display //function that displays a given string on the display
void handledDisplay::showString(const char * buf, uint8_t pos_f){ void handledDisplay::showString(const char * buf, uint8_t pos_f){
//calculate actual absolute position //calculate actual absolute position
@ -103,11 +104,11 @@ void handledDisplay::showString(const char * buf, uint8_t pos_f){
//TODO: blinkStrings() and blink() are very similar - can be optimized? //TODO: blinkStrings() and blink() are very similar - can be optimized?
//only difficulty currently is the reset behaivor of blinkStrings through showString (blink does not reset) //only difficulty is the reset behaivor of blinkStrings through showString (blink does not reset)
//---------------------------------- //==================================
//---------- blinkStrings ---------- //========== blinkStrings ==========
//---------------------------------- //==================================
//function switches between two strings in a given interval //function switches between two strings in a given interval
void handledDisplay::blinkStrings(const char * strOn_f, const char * strOff_f, uint32_t msOn_f, uint32_t msOff_f){ void handledDisplay::blinkStrings(const char * strOn_f, const char * strOff_f, uint32_t msOn_f, uint32_t msOff_f){
//copy/update variables //copy/update variables
@ -130,9 +131,9 @@ void handledDisplay::blinkStrings(const char * strOn_f, const char * strOff_f, u
//------------------------------- //===============================
//------------ blink ------------ //============ blink ============
//------------------------------- //===============================
//function triggers certain count and interval of off durations //function triggers certain count and interval of off durations
void handledDisplay::blink(uint8_t count_f, uint32_t msOn_f, uint32_t msOff_f, const char * strOff_f) { void handledDisplay::blink(uint8_t count_f, uint32_t msOn_f, uint32_t msOff_f, const char * strOff_f) {
//copy/update parameters //copy/update parameters
@ -156,9 +157,9 @@ void handledDisplay::blink(uint8_t count_f, uint32_t msOn_f, uint32_t msOff_f, c
//-------------------------------- //================================
//------------ handle ------------ //============ handle ============
//-------------------------------- //================================
//function that handles time based modes //function that handles time based modes
//writes text to the 7 segment display depending on the current mode //writes text to the 7 segment display depending on the current mode
void handledDisplay::handle() { void handledDisplay::handle() {

View File

@ -16,9 +16,8 @@ extern "C"
#include <cstring> #include <cstring>
#include "config.hpp"
//function for initializing the display using configuration from macros in config.hpp //function for initializing the display using configuration from macros in config.h
max7219_t display_init(); max7219_t display_init();
//show welcome message on the entire display //show welcome message on the entire display
@ -44,8 +43,8 @@ class handledDisplay {
//TODO: blinkStrings and blink are very similar - optimize? //TODO: blinkStrings and blink are very similar - optimize?
//TODO: add 'scroll string' method //TODO: add 'scroll string' method
private:
private:
//--- variables --- //--- variables ---
//config //config
max7219_t dev; max7219_t dev;

View File

@ -8,6 +8,8 @@ extern "C" {
} }
#include "encoder.hpp" #include "encoder.hpp"
#include "config.h"
#include "global.hpp"
//---------------------------- //----------------------------
@ -17,10 +19,14 @@ static rotary_encoder_info_t encoder; //encoder device/info
QueueHandle_t encoder_queue = NULL; //encoder event queue QueueHandle_t encoder_queue = NULL; //encoder event queue
//------------------------- //-------------------------
//------- functions ------- //------- functions -------
//------------------------- //-------------------------
//--- encoder_init ---
//======================
//==== encoder_init ====
//======================
//initialize encoder and return event queue //initialize encoder and return event queue
QueueHandle_t encoder_init(){ QueueHandle_t encoder_init(){
// esp32-rotary-encoder requires that the GPIO ISR service is installed before calling rotary_encoder_register() // esp32-rotary-encoder requires that the GPIO ISR service is installed before calling rotary_encoder_register()
@ -41,7 +47,9 @@ QueueHandle_t encoder_init(){
} }
//--- encoder_getSteps --- //========================
//=== encoder_getSteps ===
//========================
//get steps counted since last reset //get steps counted since last reset
int encoder_getSteps(){ int encoder_getSteps(){
// Poll current position and direction // Poll current position and direction
@ -52,14 +60,18 @@ int encoder_getSteps(){
} }
//--- encoder_getLenMm --- //========================
//=== encoder_getLenMm ===
//========================
//get current length in Mm since last reset //get current length in Mm since last reset
int encoder_getLenMm(){ int encoder_getLenMm(){
return (float)encoder_getSteps() * 1000 / ENCODER_STEPS_PER_METER; return (float)encoder_getSteps() * 1000 / ENCODER_STEPS_PER_METER;
} }
//--- encoder_reset --- //=======================
//==== encoder_reset ====
//=======================
//reset counted steps / length to 0 //reset counted steps / length to 0
void encoder_reset(){ void encoder_reset(){
rotary_encoder_reset(&encoder); rotary_encoder_reset(&encoder);

View File

@ -6,7 +6,6 @@ extern "C" {
#include <freertos/task.h> #include <freertos/task.h>
} }
#include "config.hpp"
//---------------------------- //----------------------------
@ -34,7 +33,6 @@ int encoder_getSteps();
int encoder_getLenMm(); int encoder_getLenMm();
//--- encoder_reset --- //--- encoder_reset ---
//reset counted steps / length to 0 //reset counted steps / length to 0
void encoder_reset(); void encoder_reset();

View File

@ -1,4 +1,5 @@
#include "config.hpp" #include "global.hpp"
#include "config.h"
//--- inputs --- //--- inputs ---

33
main/global.hpp Normal file
View File

@ -0,0 +1,33 @@
#pragma once
extern "C" {
#include "driver/adc.h"
}
#include "gpio_evaluateSwitch.hpp"
#include "buzzer.hpp"
#include "switchesAnalog.hpp"
//note: in the actual code macro variables to these objects from config.h are used as the objects names
//============================
//===== global variables =====
//============================
//create global evaluated switch objects for all available pins
//--- switches on digital gpio pins ---
//extern gpio_evaluatedSwitch sw_gpio_39;
extern gpio_evaluatedSwitch sw_gpio_34;
extern gpio_evaluatedSwitch sw_gpio_32;
extern gpio_evaluatedSwitch sw_gpio_33;
extern gpio_evaluatedSwitch sw_gpio_25;
extern gpio_evaluatedSwitch sw_gpio_26;
extern gpio_evaluatedSwitch sw_gpio_14;
//--- switches connected to 4-sw-to-analog stripboard ---
extern gpio_evaluatedSwitch sw_gpio_analog_0;
extern gpio_evaluatedSwitch sw_gpio_analog_1;
extern gpio_evaluatedSwitch sw_gpio_analog_2;
extern gpio_evaluatedSwitch sw_gpio_analog_3;
//create global buzzer object
extern buzzer_t buzzer;

View File

@ -5,170 +5,276 @@ extern "C"
#include "freertos/task.h" #include "freertos/task.h"
#include "esp_log.h" #include "esp_log.h"
#include "driver/adc.h" #include "driver/adc.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
} }
#include "stepper.hpp" #include "stepper.hpp"
#include "config.hpp" #include "config.h"
#include "global.hpp"
#include "guide-stepper.hpp" #include "guide-stepper.hpp"
#include "encoder.hpp" #include "encoder.hpp"
#include "shutdown.hpp"
//macro to get smaller value out of two
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
//--------------------- //---------------------
//--- configuration --- //--- configuration ---
//--------------------- //---------------------
//also see config.hpp //also see config.h
//for pin definition //for pin definitions and guide parameters
#define STEPPER_TEST_TRAVEL 65 //mm // configure testing modes
// #define STEPPER_TEST_TRAVEL 65 // mm
#define MIN_MM 0 // speeds for testing with potentiometer (test task only)
#define MAX_MM 60 #define SPEED_MIN 2.0 // mm/s
#define POS_MAX_STEPS MAX_MM * STEPPER_STEPS_PER_MM #define SPEED_MAX 70.0 // mm/s
#define POS_MIN_STEPS MIN_MM * STEPPER_STEPS_PER_MM //note: actual speed is currently defined in config.h with STEPPER_SPEED_DEFAULT
//simulate encoder with reset button to test stepper ctl task
//note STEPPER_TEST has to be defined as well
//#define STEPPER_SIMULATE_ENCODER
#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 #define PI 3.14159
//#define POS_MAX_STEPS GUIDE_MAX_MM * STEPPER_STEPS_PER_MM //note replaced with posMaxSteps
#define POS_MIN_STEPS GUIDE_MIN_MM * STEPPER_STEPS_PER_MM
//---------------------- //----------------------
//----- variables ------ //----- variables ------
//---------------------- //----------------------
static const char *TAG = "stepper"; //tag for logging typedef enum axisDirection_t {AXIS_MOVING_LEFT = 0, AXIS_MOVING_RIGHT} axisDirection_t;
static bool stepp_direction = true; static const char *TAG = "stepper-ctrl"; //tag for logging
static axisDirection_t currentAxisDirection = AXIS_MOVING_RIGHT;
static uint32_t posNow = 0; static uint32_t posNow = 0;
static int layerCount = 0;
// queue for sending commands to task handling guide movement
static QueueHandle_t queue_commandsGuideTask;
// mutex to prevent multiple axis to config variables also accessed/modified by control task
SemaphoreHandle_t configVariables_mutex = xSemaphoreCreateMutex();
// configured winding width: position the axis returns again in steps
static uint32_t posMaxSteps = GUIDE_MAX_MM * STEPPER_STEPS_PER_MM; //assign default width
//---------------------- //----------------------
//----- functions ------ //----- 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 //=== guide_getAxisPosSteps ===
void init_stepper() { //=============================
// ESP_LOGW(TAG, "initializing stepper..."); // return local variable posNow
// DendoStepper_config_t step_cfg = { // needed at shutdown detection to store last axis position in nvs
// .stepPin = STEPPER_STEP_PIN, int guide_getAxisPosSteps(){
// .dirPin = STEPPER_DIR_PIN, return posNow;
// .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 //=============================
//=== guide_setWindingWidth ===
//=============================
// set custom winding width (axis position the guide returns in mm)
void guide_setWindingWidth(uint8_t maxPosMm)
{
if (xSemaphoreTake(configVariables_mutex, portMAX_DELAY) == pdTRUE) // mutex to prevent multiple access by control and stepper-ctl task
{
posMaxSteps = maxPosMm * STEPPER_STEPS_PER_MM;
ESP_LOGI(TAG, "set winding width / max pos to %dmm", maxPosMm);
xSemaphoreGive(configVariables_mutex);
}
}
//=======================================
//=== guide_targetLength2WindingWidth ===
//=======================================
// calculate dynamic winding width in mm from cable length in mm
uint8_t guide_targetLength2WindingWidth(int lenMm)
{
#ifdef DYNAMIC_WINDING_WIDTH_ENABLED
uint8_t width;
//--- config ---
// define thresholds for winding widths according to target length:
if (lenMm <= 5000) // 0-5m
width = 15;
else if (lenMm <= 10000) // 6-10m
width = 25;
else if (lenMm <= 15000) // 11-15m
width = 30;
else if (lenMm <= 25000) // 16-25m
width = 65;
else // >25m
width = GUIDE_MAX_MM;
ESP_LOGW(TAG, "length2width: calculated windingWidth=%dmm from targetLength=%dm", width, lenMm);
return width;
#else
ESP_LOGD(TAG, "length2width: dynamic windingWidh not enabled, stay at GUIDE_MAX=%d", GUIDE_MAX_MM);
return GUIDE_MAX_MM;
#endif
//TODO update winding width here as well already?
}
//=============================
//=== guide_getWindingWidth ===
//=============================
// get currently configured winding width (axis position at which the guide returns in mm)
uint8_t guide_getWindingWidth()
{
if (xSemaphoreTake(configVariables_mutex, portMAX_DELAY) == pdTRUE) // mutex to prevent multiple access by control and stepper-ctl task
{
uint8_t returnValue = posMaxSteps / STEPPER_STEPS_PER_MM;
xSemaphoreGive(configVariables_mutex);
return returnValue;
}
return 0;
}
//==========================
//==== guide_moveToZero ====
//==========================
//tell stepper-control task to move cable guide to zero position
void guide_moveToZero(){
bool valueToSend = true; // or false
xQueueSend(queue_commandsGuideTask, &valueToSend, portMAX_DELAY);
ESP_LOGI(TAG, "sending command to stepper_ctl task via queue");
}
//---------------------
//---- travelSteps ----
//---------------------
//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
// cancel when width is zero or no steps received
if (posMaxSteps == 0 || stepsTarget == 0){
ESP_LOGD(TAG, "travelSteps: MaxSteps or stepsTarget = 0 -> nothing to do");
return;
}
int stepsToGo, remaining;
stepsToGo = abs(stepsTarget);
// invert direction in reverse mode (cable gets spooled off reel)
if (stepsTarget < 0) {
currentAxisDirection = (currentAxisDirection == AXIS_MOVING_LEFT) ? AXIS_MOVING_RIGHT : AXIS_MOVING_LEFT; //toggle between RIGHT<->Left
}
while (stepsToGo != 0){
//--- currently moving right ---
if (currentAxisDirection == AXIS_MOVING_RIGHT){ //currently moving right
if (xSemaphoreTake(configVariables_mutex, portMAX_DELAY) == pdTRUE) { //prevent multiple acces on posMaxSteps by control-task
remaining = posMaxSteps - posNow; //calc remaining distance fom current position to limit
if (stepsToGo > remaining){ //new distance will exceed limit
stepper_setTargetPosSteps(posMaxSteps); //move to limit
stepper_waitForStop(1000);
posNow = posMaxSteps;
currentAxisDirection = AXIS_MOVING_LEFT; //change current direction for next iteration
//increment/decrement layer count depending on current cable direction
layerCount += (stepsTarget > 0) - (stepsTarget < 0);
if (layerCount < 0) layerCount = 0; //negative layers are not possible
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)
}
xSemaphoreGive(configVariables_mutex);
}
}
//--- currently moving left ---
else if (currentAxisDirection == AXIS_MOVING_LEFT){
remaining = posNow - POS_MIN_STEPS;
if (stepsToGo > remaining){
stepper_setTargetPosSteps(POS_MIN_STEPS);
stepper_waitForStop(1000);
posNow = POS_MIN_STEPS;
currentAxisDirection = AXIS_MOVING_RIGHT; //switch direction
//increment/decrement layer count depending on current cable direction
layerCount += (stepsTarget > 0) - (stepsTarget < 0);
if (layerCount < 0) layerCount = 0; //negative layers are not possible
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;
}
}
}
// undo inversion of currentAxisDirection after reverse mode is finished
if (stepsTarget < 0) {
currentAxisDirection = (currentAxisDirection == AXIS_MOVING_LEFT) ? AXIS_MOVING_RIGHT : AXIS_MOVING_LEFT; //toggle between RIGHT<->Left
}
return;
}
//------------------
//---- travelMm ----
//------------------
//move axis certain Mm (relative) between left and right or reverse when negative
void travelMm(int length){
travelSteps(length * STEPPER_STEPS_PER_MM);
}
//----------------------
//---- init_stepper ----
//----------------------
//initialize/configure stepper instance
void init_stepper() {
//TODO unnecessary wrapper?
ESP_LOGW(TAG, "initializing stepper...");
stepper_init();
// create queue for sending commands to task handling guide movement
// currently length 1 and only one command possible, thus bool
queue_commandsGuideTask = xQueueCreate(1, sizeof(bool));
}
//--------------------------
//--- updateSpeedFromAdc ---
//--------------------------
//function that updates speed value using ADC input and configured MIN/MAX - used for testing only
void updateSpeedFromAdc() { void updateSpeedFromAdc() {
int potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095 GPIO34 int potiRead = gpio_readAdc(ADC_CHANNEL_POTI); //0-4095 GPIO34
double poti = potiRead/4095.0; double poti = potiRead/4095.0;
int speed = poti*(SPEED_MAX-SPEED_MIN) + SPEED_MIN; 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); ESP_LOGW(TAG, "poti: %d (%.2lf%%), set speed to: %d", potiRead, poti*100, speed);
} }
//---------------------------- //============================
//---- TASK stepper-test ----- //==== TASK stepper_test =====
//---------------------------- //============================
//test axis without using encoder input
#ifndef STEPPER_SIMULATE_ENCODER
void task_stepper_test(void *pvParameter) void task_stepper_test(void *pvParameter)
{ {
stepper_init(); stepper_init();
int state = 0;
while(1){ while(1){
vTaskDelay(20 / portTICK_PERIOD_MS); vTaskDelay(20 / portTICK_PERIOD_MS);
@ -183,6 +289,8 @@ void task_stepper_test(void *pvParameter)
SW_CUT.handle(); SW_CUT.handle();
SW_AUTO_CUT.handle(); SW_AUTO_CUT.handle();
#ifdef ONE_BUTTON_TEST //test with "reset-button" only
static int state = 0;
//cycle through test commands with one button //cycle through test commands with one button
if (SW_RESET.risingEdge) { if (SW_RESET.risingEdge) {
switch (state){ switch (state){
@ -208,80 +316,152 @@ void task_stepper_test(void *pvParameter)
break; break;
} }
} }
#else //test with all buttons
if (SW_RESET.risingEdge) {
buzzer.beep(1, 500, 100);
stepper_setTargetPosMm(0);
}
if (SW_PRESET1.risingEdge) {
buzzer.beep(1, 200, 100);
stepper_setTargetPosMm(50);
}
if (SW_PRESET2.risingEdge) {
buzzer.beep(2, 200, 100);
stepper_setTargetPosMm(75);
}
if (SW_PRESET3.risingEdge) {
buzzer.beep(3, 200, 100);
stepper_setTargetPosMm(100);
}
#endif
} }
// 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);
// }
} }
#endif //end SIMULATE_ENCODER
//============================
//---------------------------- //===== TASK stepper_ctl =====
//----- TASK stepper-ctl ----- //============================
//---------------------------- //task controlling the linear axis guiding the cable according to wire length spooled
#ifdef STEPPER_SIMULATE_ENCODER
void task_stepper_test(void *pvParameter)
#else
void task_stepper_ctl(void *pvParameter) void task_stepper_ctl(void *pvParameter)
#endif
{ {
// //variables //-- variables --
// int encStepsNow = 0; //get curret steps of encoder static int encStepsPrev = 0; //measured encoder steps at last run
// int encStepsPrev = 0; //steps at last check static double travelStepsPartial = 0; //store resulted remaining partial steps last run
// int encStepsDelta = 0; //steps changed since last iteration
// //temporary variables for calculating required steps, or logging
// double cableLen = 0; int encStepsNow = 0; //get curretly measured steps of encoder
// double travelStepsExact = 0; //steps axis has to travel int encStepsDelta = 0; //encoder steps changed since last iteration
// double travelStepsPartial = 0;
// int travelStepsFull = 0; double cableLen = 0;
// double travelMm = 0; double travelStepsExact = 0; //steps axis has to travel
// double turns = 0; int travelStepsFull = 0;
// double travelMm = 0;
// float potiModifier; double turns = 0;
// float currentDiameter;
// init_stepper();
// home();
//
// while(1){ //initialize stepper at task start
// //get current length init_stepper();
// encStepsNow = encoder_getSteps(); //define zero-position
// // use last known position stored at last shutdown to reduce time crashing into hardware limit
// //calculate change int posLastShutdown = nvsReadLastAxisPosSteps();
// encStepsDelta = encStepsNow - encStepsPrev; //FIXME MAJOR BUG: when resetting encoder/length in control task, diff will be huge! if (posLastShutdown >= 0)
// { // would be -1 when failed
// //read potentiometer and normalize (0-1) to get a variable for testing ESP_LOGW(TAG, "auto-home: considerting pos last shutdown %dmm + tolerance %dmm",
// potiModifier = (float) gpio_readAdc(ADC_CHANNEL_POTI) / 4095; //0-4095 -> 0-1 posLastShutdown / STEPPER_STEPS_PER_MM,
// //ESP_LOGI(TAG, "current poti-modifier = %f", potiModifier); AUTO_HOME_TRAVEL_ADD_TO_LAST_POS_MM);
// // home considering last position and offset, but limit to max distance possible
// //calculate steps to move stepper_home(MIN((posLastShutdown/STEPPER_STEPS_PER_MM + AUTO_HOME_TRAVEL_ADD_TO_LAST_POS_MM), MAX_TOTAL_AXIS_TRAVEL_MM));
// cableLen = (double)encStepsDelta * 1000 / ENCODER_STEPS_PER_METER; }
// turns = cableLen / (PI * D_REEL); else { // default to max travel when read from nvs failed
// travelMm = turns * D_CABLE; stepper_home(MAX_TOTAL_AXIS_TRAVEL_MM);
// travelStepsExact = travelMm * STEPPER_STEPS_PER_MM + travelStepsPartial; //convert mm to steps and add not moved partial steps }
// travelStepsPartial = 0;
// travelStepsFull = (int)travelStepsExact; //repeatedly read changes in measured cable length and move axis accordingly
// while(1){
// //move axis when ready to move at least 1 step
// if (abs(travelStepsFull) > 1){ // guide is disabled when posMaxSteps is zero:
// travelStepsPartial = fmod(travelStepsExact, 1); //save remaining partial steps to be added in the next iteration if (posMaxSteps == 0)
// 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); // move to starting position
// //TODO: calculate variable speed for smoother movement? for example intentionally lag behind and calculate speed according to buffered data stepper_setTargetPosSteps(0);
// //....step.setSpeedMm(35, 100, 50); vTaskDelay(1000 / portTICK_PERIOD_MS);
// //testing: get speed from poti ESP_LOGD(TAG, "posMaxSteps is zero -> guide disabled, doing nothing");
// //step.setSpeedMm(35, 1000*potiModifier+1, 1000*potiModifier+1); // stop loop iteration
// travelSteps(travelStepsExact); continue;
// encStepsPrev = encStepsNow; //update previous length }
// }
// else { #ifdef STEPPER_SIMULATE_ENCODER
// //TODO use encoder queue to only run this check at encoder event? //TESTING - simulate encoder using switch
// vTaskDelay(2); SW_RESET.handle();
// } //note
// } if (SW_RESET.risingEdge) encStepsNow += 500;
#else
//get current length
encStepsNow = encoder_getSteps();
#endif
// move to zero and reset if command received via queue
bool receivedValue;
if (xQueueReceive(queue_commandsGuideTask, &receivedValue, 0) == pdTRUE)
{
// Process the received value
// TODO support other commands (currently only move to zero possible)
ESP_LOGW(TAG, "task: move-to-zero command received via queue, starting move, waiting until position reached");
stepper_setTargetPosMm(0);
stepper_waitForStop();
//reset variables -> start tracking cable movement starting from position zero
// ensure stepsDelta is 0
encStepsNow = encoder_getSteps();
encStepsPrev = encStepsNow;
travelStepsPartial = 0;
// set locally stored axis position and counted layers to 0 (used for calculating the target axis coordinate and steps)
posNow = 0;
layerCount = 0;
currentAxisDirection = AXIS_MOVING_RIGHT;
ESP_LOGW(TAG, "at position 0, reset variables, resuming normal cable guiding operation");
}
//calculate change
encStepsDelta = encStepsNow - encStepsPrev;
//check if reset happend without moving to zero before - resulting in huge diff
if (encStepsDelta != 0 && encStepsNow == 0){ // this should not happen and causes weird movement
ESP_LOGE(TAG, "encoder steps changed to 0 (reset) without previous moveToZero() call, resulting in stepsDelta=%d", encStepsDelta);
}
//read potentiometer and normalize (0-1) to get a variable for testing
//float 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;
//effective diameter increases each layer
currentDiameter = D_REEL + LAYER_THICKNESS_MM * 2 * layerCount;
turns = cableLen / (PI * currentDiameter);
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 full step
if (abs(travelStepsFull) > 1){
travelStepsPartial = fmod(travelStepsExact, 1); //save remaining partial steps to be added in the next iteration
ESP_LOGI(TAG, "dCablelen=%.2lf, dTurns=%.2lf, travelMm=%.3lf, StepsExact: %.3lf, StepsFull=%d, StepsPartial=%.3lf, totalLayerCount=%d, diameter=%.1f", cableLen, turns, travelMm, travelStepsExact, travelStepsFull, travelStepsPartial, layerCount, currentDiameter);
ESP_LOGD(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(5);
}
vTaskDelay(5 / portTICK_PERIOD_MS);
}
} }

View File

@ -10,3 +10,21 @@ void task_stepper_test(void *pvParameter);
//task that initializes and controls the stepper motor //task that initializes and controls the stepper motor
// - moves stepper according to encoder movement // - moves stepper according to encoder movement
void task_stepper_ctl(void *pvParameter); void task_stepper_ctl(void *pvParameter);
//tell stepper-control task to move cable guide to zero position
void guide_moveToZero();
// return local variable posNow that stores the current position of cable guide axis in steps
// needed by shutdown to store last axis position in nvs
int guide_getAxisPosSteps();
// set custom winding width (axis position the guide returns in mm)
void guide_setWindingWidth(uint8_t maxPosMm);
// get currently configured winding width (axis position the guide returns in mm)
uint8_t guide_getWindingWidth();
// calculate dynamic winding width in mm from cable length in mm according to fixed thresholds
uint8_t guide_targetLength2WindingWidth(int lenMm);

View File

@ -8,14 +8,17 @@ extern "C"
#include "esp_system.h" #include "esp_system.h"
#include "esp_log.h" #include "esp_log.h"
#include "driver/adc.h" #include "driver/adc.h"
} }
#include "config.hpp" #include "config.h"
#include "global.hpp"
#include "control.hpp" #include "control.hpp"
#include "buzzer.hpp" #include "buzzer.hpp"
#include "switchesAnalog.hpp" #include "switchesAnalog.hpp"
#include "guide-stepper.hpp" #include "guide-stepper.hpp"
#include "encoder.hpp" #include "encoder.hpp"
#include "shutdown.hpp"
#include "stepper.hpp" #include "stepper.hpp"
@ -23,15 +26,20 @@ extern "C"
//================================= //=================================
//=========== functions =========== //=========== functions ===========
//================================= //=================================
//------------------------
//--- configure output --- //--- configure output ---
//function to configure gpio pin as output //------------------------
//configure a gpio pin as output
void gpio_configure_output(gpio_num_t gpio_pin){ void gpio_configure_output(gpio_num_t gpio_pin){
gpio_pad_select_gpio(gpio_pin); gpio_pad_select_gpio(gpio_pin);
gpio_set_direction(gpio_pin, GPIO_MODE_OUTPUT); gpio_set_direction(gpio_pin, GPIO_MODE_OUTPUT);
} }
//--- init gpios --- //--------------------
//---- init gpios ----
//--------------------
void init_gpios(){ void init_gpios(){
//--- outputs --- //--- outputs ---
//4x stepper mosfets //4x stepper mosfets
@ -53,6 +61,8 @@ void init_gpios(){
//initialize and configure ADC //initialize and configure ADC
adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096 adc1_config_width(ADC_WIDTH_BIT_12); //=> max resolution 4096
adc1_config_channel_atten(ADC_CHANNEL_POTI, ADC_ATTEN_DB_11); //max voltage adc1_config_channel_atten(ADC_CHANNEL_POTI, ADC_ATTEN_DB_11); //max voltage
adc1_config_channel_atten(ADC_CHANNEL_SUPPLY_VOLTAGE, ADC_ATTEN_DB_11); //max voltage
} }
@ -74,33 +84,40 @@ void task_buzzer( void * pvParameters ){
//====================================== //======================================
extern "C" void app_main() extern "C" void app_main()
{ {
//init outputs //init outputs and adc
init_gpios(); init_gpios();
//enable 5V volate regulator //enable 5V volage regulator (needed for display)
gpio_set_level(GPIO_NUM_17, 1); gpio_set_level(GPIO_NUM_17, 1);
//init encoder (global) //init encoder (global)
encoder_queue = encoder_init(); encoder_queue = encoder_init();
//define loglevel //define loglevel
esp_log_level_set("*", ESP_LOG_INFO); esp_log_level_set("*", ESP_LOG_INFO); //default loglevel
esp_log_level_set("buzzer", ESP_LOG_ERROR); esp_log_level_set("buzzer", ESP_LOG_ERROR);
esp_log_level_set("switches-analog", ESP_LOG_WARN); esp_log_level_set("switches-analog", ESP_LOG_WARN);
esp_log_level_set("control", ESP_LOG_INFO); esp_log_level_set("control", ESP_LOG_INFO);
esp_log_level_set("stepper", ESP_LOG_DEBUG); esp_log_level_set("stepper-driver", ESP_LOG_WARN);
esp_log_level_set("stepper-ctrl", ESP_LOG_WARN);
esp_log_level_set("Dendostepper", ESP_LOG_WARN); //stepper lib esp_log_level_set("Dendostepper", ESP_LOG_WARN); //stepper lib
esp_log_level_set("calc", ESP_LOG_WARN); //stepper lib esp_log_level_set("calc", ESP_LOG_WARN); //stepper lib
esp_log_level_set("lowVoltage", ESP_LOG_INFO);
#ifdef STEPPER_TEST #ifdef STEPPER_TEST
//create task for stepper testing //create task for testing the stepper motor
xTaskCreate(task_stepper_test, "task_stepper_test", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL); xTaskCreate(task_stepper_test, "task_stepper_test", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL);
//xTaskCreate(task_stepper_debug, "task_stepper_test", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL); //xTaskCreate(task_stepper_debug, "task_stepper_test", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL);
#else #else
//create task for detecting power-off
xTaskCreate(&task_shutDownDetection, "task_shutdownDet", 2048, NULL, 2, NULL);
// wait for nvs to be initialized in shutDownDetection task
vTaskDelay(50 / portTICK_PERIOD_MS);
//create task for controlling the machine //create task for controlling the machine
xTaskCreate(task_control, "task_control", configMINIMAL_STACK_SIZE * 3, NULL, 4, NULL); xTaskCreate(task_control, "task_control", configMINIMAL_STACK_SIZE * 3, NULL, 4, NULL);
//create task for controlling the stepper //create task for controlling the stepper motor (linear axis that guids the cable)
xTaskCreate(task_stepper_ctl, "task_stepper_ctl", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL); xTaskCreate(task_stepper_ctl, "task_stepper_ctl", configMINIMAL_STACK_SIZE * 3, NULL, 2, NULL);
#endif #endif

120
main/shutdown.cpp Normal file
View File

@ -0,0 +1,120 @@
extern "C"
{
#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "esp_system.h"
#include "esp_log.h"
#include "driver/adc.h"
#include "nvs_flash.h"
#include "nvs.h"
}
#include "config.h"
#include "shutdown.hpp"
#include "guide-stepper.hpp"
#define ADC_LOW_VOLTAGE_THRESHOLD 3200 // adc value where shut down is detected (store certain values before complete power loss)
static const char *TAG = "lowVoltage"; // tag for logging
nvs_handle_t nvsHandle; // access nvs that was opened once, in any function
// store a u32 value in nvs as "lastPosSteps"
void nvsWriteLastAxisPos(uint32_t value)
{
// update nvs value
esp_err_t err = nvs_set_u32(nvsHandle, "lastPosSteps", value);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
ESP_LOGW(TAG, "updated value in nvs to %d", value);
}
// read "lastPosSteps" from nvs, returns -1 if failed
int nvsReadLastAxisPosSteps()
{
uint32_t valueRead;
esp_err_t err = nvs_get_u32(nvsHandle, "lastPosSteps", &valueRead);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value %d from nvs", valueRead);
return valueRead;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGE(TAG, "nvs: the value '%s' is not initialized yet", "lastPosSteps");
return -1;
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
return -1;
}
}
// task that repeatedly checks supply voltage (12V) and saves certain values to nvs in case of power off detected
// note: with the 2200uF capacitor in the 12V supply and measuring if 12V start do drop here, there is more than enough time to take action until the 3v3 regulator turns off
void task_shutDownDetection(void *pvParameter)
{
//--- Initialize NVS ---
ESP_LOGW(TAG, "initializing nvs...");
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_LOGE(TAG, "NVS truncated -> deleting flash");
// Retry nvs_flash_init
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
//--- open nvs-flash ---
ESP_LOGW(TAG, "opening NVS-handle...");
// create handle available for all functions in this file
err = nvs_open("storage", NVS_READWRITE, &nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
// read stored value (returns 0 if unitialized/failed)
//int lastPosSteps = nvsReadLastAxisPosSteps();
//ESP_LOGW(TAG, "=> read value %d from nvs (stored at last shutdown)", lastPosSteps);
// repeatedly read ADC and check if below low voltage threshold
bool voltageBelowThreshold = false;
while (1) //TODO limit save frequency in case voltage repeadedly varys between threshold for some reason (e.g. offset drift)
{
// read adc
int adc_reading = adc1_get_raw(ADC_CHANNEL_SUPPLY_VOLTAGE);
// evaulate threshold
if (adc_reading < ADC_LOW_VOLTAGE_THRESHOLD) // below threshold => POWER SHUTDOWN DETECTED
{
// write to nvs and log once at change to below
if (!voltageBelowThreshold){
nvsWriteLastAxisPos(guide_getAxisPosSteps());
ESP_LOGE(TAG, "voltage now below threshold! now=%d threshold=%d -> wrote last axis-pos to nvs", adc_reading, ADC_LOW_VOLTAGE_THRESHOLD);
voltageBelowThreshold = true;
}
}
else if (voltageBelowThreshold) // above threshold and previously below
{
// log at change to above
ESP_LOGE(TAG, "voltage above threshold again: %d > %d - issue with power supply, or too threshold too high?", adc_reading, ADC_LOW_VOLTAGE_THRESHOLD);
voltageBelowThreshold = false;
}
// always log for debugging/calibrating
ESP_LOGD(TAG, "read adc battery voltage: %d", adc_reading);
vTaskDelay(30 / portTICK_PERIOD_MS);
}
}

7
main/shutdown.hpp Normal file
View File

@ -0,0 +1,7 @@
// task that repeatedly checks supply voltage (12V) and saves certain values to nvs in case of it drops below a certain threshold (power off detected)
void task_shutDownDetection(void *pvParameter);
// read last axis position in steps from nvs
// returns -1 when reading from nvs failed
int nvsReadLastAxisPosSteps();

View File

@ -1,5 +1,6 @@
//custom driver for stepper motor //custom driver for stepper motor
#include "config.hpp" #include "config.h"
#include "global.hpp"
#include "hal/timer_types.h" #include "hal/timer_types.h"
#include <cstdint> #include <cstdint>
#include <inttypes.h> #include <inttypes.h>
@ -10,16 +11,19 @@ extern "C" {
#include "esp_log.h" #include "esp_log.h"
} }
//config from config.hpp
//=====================
//=== configuration ===
//=====================
//used macros from config.h:
//#define STEPPER_STEP_PIN GPIO_NUM_18 //mos1 //#define STEPPER_STEP_PIN GPIO_NUM_18 //mos1
//#define STEPPER_DIR_PIN GPIO_NUM_16 //ST3 //#define STEPPER_DIR_PIN GPIO_NUM_16 //ST3
#define STEPPER_STEPS_PER_MM 200/2 //steps/mm //#define STEPPER_STEPS_PER_MM 200/2 //steps/mm (steps-per-rot / slope)
#define STEPPER_SPEED_DEFAULT 20 //mm/s //#define STEPPER_SPEED_DEFAULT 20 //mm/s
#define STEPPER_SPEED_MIN 4 //mm/s - speed at which stepper immediately starts/stops //#define STEPPER_SPEED_MIN 4 //mm/s - speed threshold at which stepper immediately starts/stops
#define STEPPER_ACCEL_INC 3 //steps/s per cycle //#define STEPPER_ACCEL_INC 3 //steps/s increment per cycle
#define STEPPER_DECEL_INC 8 //steps/s per cycle //#define STEPPER_DECEL_INC 8 //steps/s decrement per cycle
#define TIMER_F 1000000ULL #define TIMER_F 1000000ULL
#define TICK_PER_S TIMER_S #define TICK_PER_S TIMER_S
@ -30,7 +34,7 @@ extern "C" {
//======================== //========================
//=== global variables === //=== global variables ===
//======================== //========================
static const char *TAG = "stepper-ctl"; //tag for logging static const char *TAG = "stepper-driver"; //tag for logging
bool direction = 1; bool direction = 1;
bool directionTarget = 1; bool directionTarget = 1;
@ -91,7 +95,7 @@ void task_stepper_debug(void *pvParameter){
//===== set speed ===== //===== set speed =====
//===================== //=====================
void stepper_setSpeed(uint32_t speedMmPerS) { void stepper_setSpeed(uint32_t speedMmPerS) {
ESP_LOGW(TAG, "set target speed from %u to %u mm/s (%u steps/s)", ESP_LOGI(TAG, "set target speed from %u to %u mm/s (%u steps/s)",
speedTarget, speedMmPerS, speedMmPerS * STEPPER_STEPS_PER_MM); speedTarget, speedMmPerS, speedMmPerS * STEPPER_STEPS_PER_MM);
speedTarget = speedMmPerS * STEPPER_STEPS_PER_MM; speedTarget = speedMmPerS * STEPPER_STEPS_PER_MM;
} }
@ -102,14 +106,14 @@ void stepper_setSpeed(uint32_t speedMmPerS) {
//== set target pos STEPS == //== set target pos STEPS ==
//========================== //==========================
void stepper_setTargetPosSteps(uint64_t target_steps) { void stepper_setTargetPosSteps(uint64_t target_steps) {
ESP_LOGW(TAG, "update target position from %llu to %llu steps (stepsNow: %llu", posTarget, target_steps, posNow); ESP_LOGI(TAG, "update target position from %llu to %llu steps (stepsNow: %llu", posTarget, target_steps, posNow);
posTarget = target_steps; posTarget = target_steps;
// Check if the timer is currently paused // Check if the timer is currently paused
if (!timerIsRunning){ if (!timerIsRunning){
// If the timer is paused, start it again with the updated targetSteps // If the timer is paused, start it again with the updated targetSteps
timerIsRunning = true; timerIsRunning = true;
ESP_LOGW(TAG, "starting timer because did not run before"); ESP_LOGI(TAG, "starting timer");
ESP_ERROR_CHECK(timer_set_alarm_value(timerGroup, timerIdx, 1000)); ESP_ERROR_CHECK(timer_set_alarm_value(timerGroup, timerIdx, 1000));
//timer_set_counter_value(timerGroup, timerIdx, 1000); //timer_set_counter_value(timerGroup, timerIdx, 1000);
ESP_ERROR_CHECK(timer_start(timerGroup, timerIdx)); ESP_ERROR_CHECK(timer_start(timerGroup, timerIdx));
@ -122,24 +126,67 @@ void stepper_setTargetPosSteps(uint64_t target_steps) {
//=== set target pos MM === //=== set target pos MM ===
//========================= //=========================
void stepper_setTargetPosMm(uint32_t posMm){ void stepper_setTargetPosMm(uint32_t posMm){
ESP_LOGW(TAG, "set target position to %u mm", posMm); ESP_LOGI(TAG, "set target position to %u mm", posMm);
stepper_setTargetPosSteps(posMm * STEPPER_STEPS_PER_MM); stepper_setTargetPosSteps(posMm * STEPPER_STEPS_PER_MM);
} }
//=======================
//===== waitForStop =====
//=======================
//delay until stepper is stopped, optional timeout in ms, 0 = no limit
void stepper_waitForStop(uint32_t timeoutMs){
ESP_LOGI(TAG, "waiting for stepper to stop...");
uint32_t timestampStart = esp_log_timestamp();
while (timerIsRunning) {
if ( (esp_log_timestamp() - timestampStart) >= timeoutMs && timeoutMs != 0){
ESP_LOGE(TAG, "timeout waiting for stepper to stop");
return;
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
ESP_LOGI(TAG, "finished waiting stepper to stop");
return;
}
//======================
//======== home ========
//======================
//define zero/start position
//run to limit and define zero/start position.
//Currently simply runs stepper for travelMm and bumps into hardware limit
void stepper_home(uint32_t travelMm){
//TODO add timeout, limitswitch...
ESP_LOGW(TAG, "initiate auto-home, moving %d mm...", travelMm);
posNow = travelMm * STEPPER_STEPS_PER_MM;
while (posNow != 0){
//reactivate just in case stopped by other call to prevent deadlock
if (!timerIsRunning) {
stepper_setTargetPosSteps(0);
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
ESP_LOGW(TAG, "finished auto-home");
return;
}
//======================== //========================
//===== init stepper ===== //===== init stepper =====
//======================== //========================
void stepper_init(){ void stepper_init(){
ESP_LOGW(TAG, "init - configure struct..."); ESP_LOGI(TAG, "init - configure struct...");
// Configure pulse and direction pins as outputs // Configure pulse and direction pins as outputs
ESP_LOGW(TAG, "init - configure gpio pins..."); ESP_LOGI(TAG, "init - configure gpio pins...");
gpio_set_direction(STEPPER_DIR_PIN, GPIO_MODE_OUTPUT); gpio_set_direction(STEPPER_DIR_PIN, GPIO_MODE_OUTPUT);
gpio_set_direction(STEPPER_STEP_PIN, GPIO_MODE_OUTPUT); gpio_set_direction(STEPPER_STEP_PIN, GPIO_MODE_OUTPUT);
ESP_LOGW(TAG, "init - initialize/configure timer..."); ESP_LOGI(TAG, "init - initialize/configure timer...");
timer_config_t timer_conf = { timer_config_t timer_conf = {
.alarm_en = TIMER_ALARM_EN, // we need alarm .alarm_en = TIMER_ALARM_EN, // we need alarm
.counter_en = TIMER_PAUSE, // dont start now lol .counter_en = TIMER_PAUSE, // dont start now lol
@ -193,8 +240,16 @@ bool timer_isr(void *arg) {
//FIXME noticed crash: division by 0 when min speed > target speed //FIXME noticed crash: division by 0 when min speed > target speed
uint64_t stepsDecelRemaining = (speedNow - speedMin) / decel_increment; uint64_t stepsDecelRemaining = (speedNow - speedMin) / decel_increment;
//DECELERATE //DECELERATE
//prevent hard stop (faster stop than decel ramp)
//Idea: when target gets lowered while decelerating,
// move further than target to not exceed decel ramp (overshoot),
// then change dir and move back to actual target pos
if (stepsToGo < stepsDecelRemaining/2){ //significantly less steps planned to comply with decel ramp
stepsToGo = stepsDecelRemaining; //set to required steps
}
if (stepsToGo <= stepsDecelRemaining) { if (stepsToGo <= stepsDecelRemaining) {
//FIXME if stepsToGo gets updated (lowered) close to target while close to target, the stepper may stop too fast -> implement possibility to 'overshoot and reverse'?
if ((speedNow - speedMin) > decel_increment) { if ((speedNow - speedMin) > decel_increment) {
speedNow -= decel_increment; speedNow -= decel_increment;
} else { } else {

View File

@ -2,12 +2,24 @@
//init stepper pins and timer //init stepper pins and timer
void stepper_init(); void stepper_init();
//delay until stepper is stopped, optional timeout in ms, 0 = no limit
void stepper_waitForStop(uint32_t timeoutMs = 0);
//run to limit and define zero/start position. (busy until finished)
//Currently simply runs stepper for travelMm and bumps into hardware limit
void stepper_home(uint32_t travelMm = 60);
//set absolute target position in steps //set absolute target position in steps
void stepper_setTargetPosSteps(uint64_t steps); void stepper_setTargetPosSteps(uint64_t steps);
//set absolute target position in millimeters //set absolute target position in millimeters
void stepper_setTargetPosMm(uint32_t posMm); void stepper_setTargetPosMm(uint32_t posMm);
//set target speed in millimeters per second //set target speed in millimeters per second
void stepper_setSpeed(uint32_t speedMmPerS); void stepper_setSpeed(uint32_t speedMmPerS);
//task that periodically logs variables for debugging stepper driver //task that periodically logs variables for debugging stepper driver
void task_stepper_debug(void *pvParameter); void task_stepper_debug(void *pvParameter);

View File

@ -1,4 +1,6 @@
#include "switchesAnalog.hpp" #include "switchesAnalog.hpp"
#include "config.h"
#include "global.hpp"
#define CHECK_BIT(var,pos) (((var)>>(pos)) & 1) //TODO duplicate code: same macro already used in vfd.cpp #define CHECK_BIT(var,pos) (((var)>>(pos)) & 1) //TODO duplicate code: same macro already used in vfd.cpp

View File

@ -8,7 +8,6 @@ extern "C"
#include <math.h> #include <math.h>
} }
#include "config.hpp"
#include "gpio_adc.hpp" #include "gpio_adc.hpp"

View File

@ -1,4 +1,6 @@
#include "vfd.hpp" #include "vfd.hpp"
#include "config.h"
#include "global.hpp"
#define CHECK_BIT(var,pos) (((var)>>(pos)) & 1) #define CHECK_BIT(var,pos) (((var)>>(pos)) & 1)

View File

@ -9,7 +9,6 @@ extern "C"
#include "esp_log.h" #include "esp_log.h"
} }
#include "config.hpp"
//enum defining motor direction //enum defining motor direction
typedef enum vfd_direction_t {FWD, REV} vfd_direction_t; typedef enum vfd_direction_t {FWD, REV} vfd_direction_t;

View File

@ -415,7 +415,6 @@ CONFIG_ESP_MAC_ADDR_UNIVERSE_ETH=y
# CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_TWO is not set # CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_TWO is not set
CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_FOUR=y CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_FOUR=y
CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES=4 CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES=4
# CONFIG_ESP_MAC_IGNORE_MAC_CRC_ERROR is not set
# end of MAC Config # end of MAC Config
# #