├── .gitattributes ├── .github └── workflows │ └── tg-send.yml ├── LICENSE ├── Planner Simulation ├── Planner │ ├── Planner.pde │ ├── button_cl.pde │ ├── fifo_cl.pde │ ├── planner_v3.pde │ ├── plotter_cl.pde │ ├── stepper_cl.pde │ └── timer_cl.pde └── Planner2 │ ├── Planner2.pde │ ├── button_cl.pde │ ├── fifo_cl.pde │ ├── planner_v4.pde │ ├── plotter_cl.pde │ ├── stepper_cl.pde │ └── timer_cl.pde ├── README.md ├── README_EN.md ├── docs ├── a4988.jpg ├── drv8825_2.jpg ├── tb6560.jpg ├── tmc2208.jpg └── uln2003.jpg ├── examples ├── Planner │ ├── PlannerArray │ │ └── PlannerArray.ino │ ├── PlannerCircle │ │ ├── PlannerCircle.ino │ │ └── stepperPlot │ │ │ └── stepperPlot.pde │ ├── PlannerControl │ │ └── PlannerControl.ino │ ├── PlannerDemo │ │ └── PlannerDemo.ino │ ├── PlannerHoming │ │ └── PlannerHoming.ino │ ├── PlannerSpeedControl │ │ └── PlannerSpeedControl.ino │ └── PlannerTimerISR │ │ ├── PlannerTimerISR.ino │ │ └── timer.ino ├── Planner2 │ ├── PlannerArray │ │ ├── PlannerArray.ino │ │ └── stepperPlot │ │ │ └── stepperPlot.pde │ ├── PlannerCircle │ │ ├── PlannerCircle.ino │ │ └── stepperPlot │ │ │ └── stepperPlot.pde │ └── PlannerCircleISR │ │ ├── PlannerCircleISR.ino │ │ ├── stepperPlot │ │ └── stepperPlot.pde │ │ └── timer.ino ├── Stepper │ ├── accelDeccelButton │ │ └── accelDeccelButton.ino │ ├── demo │ │ └── demo.ino │ ├── endSwitch │ │ └── endSwitch.ino │ ├── externalDriver │ │ └── externalDriver.ino │ ├── multiStepper │ │ └── multiStepper.ino │ ├── potPos │ │ └── potPos.ino │ ├── potSpeed │ │ └── potSpeed.ino │ ├── smoothAlgorithm │ │ └── smoothAlgorithm.ino │ ├── speed │ │ └── speed.ino │ ├── speedSerialControl │ │ └── speedSerialControl.ino │ ├── stop │ │ └── stop.ino │ ├── sweep │ │ └── sweep.ino │ └── timerISR │ │ └── timerISR.ino └── Stepper2 │ ├── SpeedControl │ └── SpeedControl.ino │ ├── StepperControl │ └── StepperControl.ino │ ├── homing │ └── homing.ino │ ├── sweep │ └── sweep.ino │ ├── sweepISR │ ├── sweepISR.ino │ └── timer.ino │ └── sweep_FAST_PROFILE │ └── sweep_FAST_PROFILE.ino ├── keywords.txt ├── library.properties └── src ├── FIFO.h ├── GStypes.h ├── GyverPlanner.h ├── GyverPlanner2.h ├── GyverStepper.h ├── GyverStepper2.h └── StepperCore.h /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.github/workflows/tg-send.yml: -------------------------------------------------------------------------------- 1 | 2 | name: Telegram Message 3 | on: 4 | release: 5 | types: [published] 6 | jobs: 7 | build: 8 | name: Send Message 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: send telegram message on push 12 | uses: appleboy/telegram-action@master 13 | with: 14 | to: ${{ secrets.TELEGRAM_TO }} 15 | token: ${{ secrets.TELEGRAM_TOKEN }} 16 | disable_web_page_preview: true 17 | message: | 18 | ${{ github.event.repository.name }} v${{ github.event.release.tag_name }} 19 | ${{ github.event.release.body }} 20 | https://github.com/${{ github.repository }} 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 AlexGyver 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/Planner.pde: -------------------------------------------------------------------------------- 1 | // СИМУЛЯЦИЯ ПЛАНИРОВЩИКА GPLANNER2 2 | 3 | int type = 2; // тип маршрута: 0 - ручной массив, 1 - случайный, 2 - окружность 4 | int dots = 20; // количество точек в маршруте 5 | 6 | int mul = 1; // уменьшить разрешение по шагам (например 6) 7 | boolean show = false; // показать сетку текущего разрешения 8 | 9 | // массив маршрута тип 0 10 | int points[][] = { 11 | {100, 0}, 12 | {120, 20}, 13 | {150, 50}, 14 | {220, 120}, 15 | }; 16 | 17 | // 18 | int WX = 800; 19 | int HX = 800; 20 | 21 | Planner_v3 pln = new Planner_v3(2); 22 | 23 | Plotter plotterX = new Plotter(0, HX/2, WX/2-10, HX/2-10, 2, 2); 24 | Plotter plotterY = new Plotter(WX/2, HX/2, WX/2-10, HX/2-10, 2, 2); 25 | 26 | Timer timer = new Timer(10); 27 | 28 | MyBtn pause = new MyBtn(false, 10, 10, 80, 20, "pause"); 29 | MyBtn resume = new MyBtn(false, 10, 30, 80, 20, "resume"); 30 | MyBtn stop = new MyBtn(false, 10, 50, 80, 20, "stop"); 31 | MyBtn brake = new MyBtn(false, 10, 70, 80, 20, "brake"); 32 | MyBtn next = new MyBtn(false, 10, 90, 80, 20, "next"); 33 | 34 | void settings() { 35 | size(WX, HX); 36 | smooth(8); 37 | } 38 | 39 | void setup() { 40 | frameRate(1000); 41 | colorMode(HSB); 42 | background(210); 43 | randomSeed(6); 44 | 45 | // забиваем маршрут 46 | if (type == 1) { 47 | points = new int[dots][4]; 48 | for (int i = 0; i < dots; i++) { 49 | points[i][0] = int(random(0, width)); 50 | points[i][1] = int(random(0, height / 2)); 51 | } 52 | points[dots-1][2] = 1; 53 | } else if (type == 2) { 54 | points = new int[dots][4]; 55 | for (int i = 0; i < dots; i++) { 56 | float radius = 180; 57 | points[i][0] = width/2+int(radius*cos((float)i/dots*2*PI)); 58 | points[i][1] = height/4+int(radius*sin((float)i/dots*2*PI)); 59 | } 60 | points[dots-1][2] = 1; 61 | } 62 | 63 | // даунскейлинг если надо 64 | for (int i = 0; i < points.length; i++) { 65 | points[i][0] /= mul; 66 | points[i][1] /= mul; 67 | } 68 | 69 | // начальные условия 70 | pln.setCurrent(points[0]); 71 | pln.setMaxSpeed(200); 72 | pln.setAcceleration(300); 73 | 74 | // запускаем плотер 75 | plotterX.init(); 76 | plotterY.init(); 77 | plotterX.showMinMax(true); 78 | plotterY.showMinMax(true); 79 | plotterY.setLineAmount(10); 80 | plotterX.setLineAmount(10); 81 | plotterX.autoScale(false); // выключим авто масштаб (по умолч. включен) 82 | plotterX.setMin(0); // минимум для фикс масштаба 83 | plotterX.setMax(300); // максимум для фикс масштаба 84 | plotterY.autoScale(false); // выключим авто масштаб (по умолч. включен) 85 | plotterY.setMin(0); // минимум для фикс масштаба 86 | plotterY.setMax(300); // максимум для фикс масштаба 87 | timer.start(); 88 | 89 | // рисуем путь 90 | drawPath(show); 91 | } 92 | 93 | int count = 0; 94 | void draw() { 95 | if (pause.pool()) pln.pause(); 96 | if (stop.pool()) pln.stop(); 97 | if (resume.pool()) pln.resume(); 98 | if (brake.pool()) pln.brake(); 99 | if (next.pool()) { 100 | int[] tar = {0,0}; 101 | if (count < points.length) pln.setTarget(tar); 102 | } 103 | 104 | if (timer.tick()) { 105 | plotterX.add(0, pln.stepper[0].pos / 5); 106 | plotterX.add(1, pln.us != 0 ? int(500000/pln.us) : 0); 107 | plotterX.update(); 108 | 109 | plotterY.add(0, pln.stepper[1].pos / 5); 110 | plotterY.add(1, pln.us != 0 ? int(500000/pln.us) : 0); 111 | plotterY.update(); 112 | } 113 | 114 | pln.tick(); 115 | if (pln.ready()) { // приехали 116 | if (count < points.length) pln.setTarget(points[count]); 117 | count++; 118 | } 119 | } 120 | 121 | void drawPath(boolean grid) { 122 | stroke(150); 123 | if (grid) { 124 | for (int i = 0; i < 200; i++) { 125 | line(i*mul, 0, i*mul, height); 126 | line(0, i*mul, width, i*mul); 127 | } 128 | } 129 | for (int i = 0; i < points.length; i++) { 130 | fill(0); 131 | noStroke(); 132 | circle(points[i][0]*mul, points[i][1]*mul, 8); 133 | stroke(0); 134 | strokeWeight(0.5); 135 | if (i > 0) line(points[i-1][0]*mul, points[i-1][1]*mul, points[i][0]*mul, points[i][1]*mul); 136 | text(i, points[i][0]*mul+10, points[i][1]*mul); 137 | //text(points[i][3], points[i][0]*mul+10, points[i][1]*mul+12); 138 | } 139 | noStroke(); 140 | } 141 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/button_cl.pde: -------------------------------------------------------------------------------- 1 | // type 0 - momentary, type 1 - fixed 2 | class MyBtn { 3 | MyBtn(boolean type, int x, int y, int a, int b, String text) { 4 | _type = type; 5 | _x = x; 6 | _y = y; 7 | _a = a; 8 | _b = b; 9 | _c1 = 180; 10 | _c2 = 150; 11 | _c3 = 120; 12 | _c4 = 0; 13 | _text = text; 14 | } 15 | MyBtn(boolean type, int x, int y, int a, int b, color c1, color c2, color c3, String text, color c4) { 16 | _type = type; 17 | _x = x; 18 | _y = y; 19 | _a = a; 20 | _b = b; 21 | _c1 = c1; 22 | _c2 = c2; 23 | _c3 = c3; 24 | _c4 = c4; 25 | _text = text; 26 | } 27 | boolean pool() { 28 | boolean flag = false; 29 | if ((mouseX > _x) && (mouseX < (_x+_a)) && (mouseY > _y) && (mouseY < (_y+_b))) { 30 | if (mousePressed) { 31 | if (!_flag) { 32 | if (_type) _state = !_state; 33 | else { 34 | flag = _state = true; 35 | } 36 | _flag = true; 37 | } 38 | fill(_c3); 39 | } else { 40 | if (_flag) _flag = false; 41 | fill(_c2); 42 | } 43 | } else { 44 | fill(_c1); 45 | if (_flag) _flag = false; 46 | } 47 | if (_state && _type) fill(_c3); 48 | stroke(0); 49 | strokeWeight(1); 50 | rect(_x, _y, _a, _b); 51 | noStroke(); 52 | fill(_c4); 53 | textSize(15); 54 | text(_text, _x+5, _y+15); 55 | return flag; 56 | } 57 | boolean check() { 58 | if (_type) return _state; 59 | else { 60 | if (_state) { 61 | _state = false; 62 | return true; 63 | } 64 | } 65 | return false; 66 | } 67 | int _x, _y, _a, _b; 68 | color _c1, _c2, _c3, _c4; 69 | String _text; 70 | boolean _state = false, _flag = false, _type = false; 71 | }; 72 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/fifo_cl.pde: -------------------------------------------------------------------------------- 1 | class FIFO { 2 | FIFO(int nsize) { 3 | size = nsize; 4 | clear(); 5 | buf = new int[size]; 6 | } 7 | 8 | void clear() { 9 | head = tail = 0; 10 | } 11 | 12 | int amount() { 13 | return (size + head - tail) % size; 14 | } 15 | 16 | int get(int i) { 17 | return buf[(tail + i) % size]; 18 | } 19 | 20 | void add(int p) { 21 | int i = (head + 1) % size; 22 | if (i != tail) { 23 | buf[head] = p; 24 | head = i; 25 | } 26 | } 27 | 28 | void set(int p) { 29 | buf[tail] = p; 30 | } 31 | 32 | void next() { 33 | tail = (tail + 1) % size; 34 | } 35 | 36 | boolean availableForWrite() { 37 | return ((head + 1) % size != tail); 38 | } 39 | 40 | int buf[]; 41 | int size, head, tail; 42 | } 43 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/planner_v3.pde: -------------------------------------------------------------------------------- 1 | class Planner_v3 { 2 | Planner_v3(int numAxles) { 3 | axles = numAxles; 4 | stepper = new Stepper[numAxles]; 5 | for (int i = 0; i < axles; i++) stepper[i] = new Stepper(); 6 | dS = new int[numAxles]; 7 | tar = new int[numAxles]; 8 | nd = new int[numAxles]; 9 | sn = new int[numAxles]; 10 | } 11 | 12 | void setMaxSpeed(int newV) { 13 | V = newV; 14 | minUs = 1000000 / V; 15 | } 16 | 17 | void setAcceleration(int newA) { 18 | a = newA; 19 | if (a != 0) us0 = int(0.676 * 1000000 * sqrt(2.0 / a)); 20 | else us0 = 0; 21 | } 22 | 23 | void pause() { 24 | status = 2; 25 | } 26 | 27 | void stop() { 28 | if (us == 0 || status != 1) return; // мы и так уже остановились, успокойся 29 | if (a == 0) { // нет ускорения - дёргай ручник 30 | brake(); 31 | return; 32 | } 33 | if (step > s2) { // а мы уже тормозим! 34 | pause(); // значит флаг на паузу 35 | return; 36 | } 37 | status = 4; 38 | step = 0; 39 | } 40 | 41 | void brake() { 42 | status = 0; 43 | us = 0; 44 | } 45 | 46 | void resume() { 47 | setTarget(tar); 48 | } 49 | 50 | void setCurrent(int cur[]) { 51 | for (int i = 0; i < axles; i++) stepper[i].pos = cur[i]; 52 | } 53 | 54 | int getCurrent(int axis) { 55 | return stepper[axis].pos; 56 | } 57 | 58 | int getTarget(int axis) { 59 | return tar[axis]; 60 | } 61 | 62 | void setTarget(int target[]) { 63 | S = 0; 64 | for (int i = 0; i < axles; i++) { // для всех осей 65 | if (status != 2) tar[i] = target[i]; // запоминаем цель, если она не остановка 66 | dS[i] = abs(target[i] - stepper[i].pos); // модуль ошибки по оси 67 | sn[i] = stepper[i].pos < target[i] ? 1 : -1; // направление движения по оси 68 | if (dS[i] > S) { 69 | S = dS[i]; 70 | maxAx = i; 71 | } 72 | } 73 | for (int i = 0; i < axles; i++) nd[i] = S / 2; // записываем половину диагонали 74 | 75 | if (S == 0) { // путь == 0, мы никуда не едем 76 | readyF = true; // готовы к следующей точке 77 | status = 0; // стоп машина 78 | return; 79 | } 80 | 81 | if (a > 0) { // ускорение задано 82 | if (us > 0) { // мы движемся! ААА! 83 | int v1 = 1000000 / us; 84 | if (2.0*V*V-v1*v1 > 2.0*a*S) { // треугольник 85 | s1 = int((2.0*a*S - v1*v1) / (4.0*a)); 86 | s2 = s1; 87 | } else { // трапеция 88 | s1 = (V*V - v1*v1) / (2 * a); 89 | s2 = S - V*V / (2 * a); 90 | } 91 | so1 = v1 * v1 / (2 * a); 92 | } else { // не движемся 93 | if (V*V > a*S) { // треугольник 94 | s1 = S / 2; 95 | s2 = s1; 96 | } else { // трапеция 97 | s1 = V*V / (2 * a); 98 | s2 = S - s1; 99 | } 100 | so1 = 0; 101 | us = us0; 102 | } 103 | } else { // ускорение отключено 104 | s1 = 0; 105 | s2 = S; 106 | us = minUs; 107 | } 108 | 109 | tmr = millis(); 110 | step = 0; 111 | readyF = false; 112 | status = 1; 113 | } 114 | 115 | // вернёт false если мотор приехал 116 | boolean tick() { 117 | if (status > 0 && millis() - tmr >= us/1000) { 118 | 119 | // ВЫВОД МАРШРУТА 120 | int vel = us==0 ? 0 : 1000000/us; 121 | int hue = int(map(vel, 0, V, 0, 70)); 122 | fill(hue, 255, 255); 123 | circle(stepper[0].pos*mul, stepper[1].pos*mul, 4); 124 | // ВЫВОД МАРШРУТА 125 | 126 | tmr = millis(); 127 | step++; 128 | for (int i = 0; i < axles; i++) { 129 | // http://members.chello.at/easyfilter/bresenham.html 130 | if (i == maxAx) stepper[i].step(sn[i]); 131 | else { 132 | nd[i] -= dS[i]; 133 | if (nd[i] < 0) { 134 | nd[i] += S; 135 | stepper[i].step(sn[i]); // двигаем мотор в направлении sn 136 | } 137 | } 138 | } 139 | 140 | if (status == 4) { 141 | us += 2 * us / (4 * (s1-step) + 1); // торможение 142 | if (step >= S || us >= us0) { 143 | us = 0; 144 | status = 0; 145 | return false; 146 | } 147 | return false; 148 | } 149 | 150 | // https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/ 151 | if (step < s1) us -= 2 * us / (4 * (step + so1) + 1); // разгон 152 | else if (step < s2) us = minUs; // постоянная 153 | else if (step < S) us += 2 * us / (4 * (S - step) + 1); // торможение 154 | else { // мы приехали 155 | us = 0; 156 | if (status == 1) readyF = true; 157 | status = 0; 158 | return false; 159 | } 160 | us = max(us, minUs); 161 | } 162 | return status > 0; 163 | } 164 | 165 | boolean ready() { 166 | return (readyF && status == 0); // если мы приехали и остановились 167 | } 168 | 169 | int[] nd, sn; 170 | int axles; 171 | int S, V, a; 172 | int s1, s2, so1; 173 | int[] dS, tar; 174 | int us0, us = 0, tmr, minUs, step; 175 | byte status = 0; 176 | int maxAx; 177 | boolean readyF = true; 178 | 179 | Stepper[] stepper; 180 | }; 181 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/plotter_cl.pde: -------------------------------------------------------------------------------- 1 | // простая библиотек плоттера для Processing 2 | // v1.0 - релиз 3 | 4 | class Plotter { 5 | // x, y, size x, size y, axes, resolution 6 | Plotter(int nx, int ny, int nxx, int nyy, int naxes, int nres) { 7 | x = nx; 8 | xx = nxx; 9 | y = ny; 10 | yy = nyy; 11 | res = nres; 12 | axes = naxes; 13 | size = xx / nres; 14 | buffer = new int[axes][size]; 15 | } 16 | 17 | // автомасштаб 18 | void autoScale(boolean nscale) { 19 | scale = nscale; 20 | } 21 | 22 | // минимум (для фикс масштаба) 23 | void setMin(int nminV) { 24 | minV = nminV; 25 | } 26 | 27 | // максимум (для фикс масштаба) 28 | void setMax(int nmaxV) { 29 | maxV = nmaxV; 30 | } 31 | 32 | // кол-во делений 33 | void setLineAmount(int nlnum) { 34 | lnum = nlnum; 35 | } 36 | 37 | // отображение минимума и максимума 38 | void showMinMax(boolean nminmax) { 39 | minmax = nminmax; 40 | } 41 | 42 | // инициализация 43 | void init() { 44 | plot = createGraphics(xx+frame*2, yy+frame*2); 45 | plot.beginDraw(); 46 | plot.textSize(11); 47 | plot.colorMode(HSB, 255, 255, 255); 48 | plot.background(0, 0); 49 | plot.endDraw(); 50 | canvas = createGraphics(xx+frame*2, yy+frame*2); 51 | canvas.beginDraw(); 52 | canvas.colorMode(HSB, 255, 255, 255); 53 | canvas.fill(255); 54 | canvas.stroke(0); 55 | canvas.strokeWeight(frame); 56 | canvas.rect(frame, frame, xx, yy); 57 | canvas.strokeWeight(1); 58 | for (int i = 0; i < axes; i++) { 59 | canvas.fill((i * hueStep) % 255, 255, 200); 60 | canvas.rect(8 + i * 16, 8, 12, 12); 61 | } 62 | canvas.endDraw(); 63 | image(canvas, x, y); 64 | } 65 | 66 | // добавить точку (ось, значение) 67 | void add(int addr, float val) { 68 | add(addr, int(val)); 69 | } 70 | 71 | // добавить точку (ось, значение) 72 | void add(int addr, int val) { 73 | plot.beginDraw(); 74 | plot.strokeWeight(1.2); 75 | plot.stroke((addr * hueStep) % 255, 255, 200); 76 | buffer[addr][count % size] = val; 77 | 78 | tmaxV = maxV; 79 | tminV = minV; 80 | int buf[] = new int [size]; 81 | 82 | tmaxV = -0xFFFFFF; 83 | tminV = 0xFFFFFF; 84 | for (int a = 0; a < axes; a++) { 85 | for (int i = 0; i < size; i++) { 86 | tmaxV = max(tmaxV, buffer[a][i]); 87 | tminV = min(tminV, buffer[a][i]); 88 | } 89 | } 90 | 91 | if (!scale) { 92 | for (int i = 0; i < size; i++) { 93 | buf[i] = constrain(buffer[addr][i], minV, maxV); 94 | } 95 | } 96 | 97 | if (scale) delta = abs(tmaxV) + abs(tminV); 98 | else delta = abs(maxV) + abs(minV); 99 | for (int i = 0; i < size; i++) { 100 | if (delta != 0) buf[i] = int((buffer[addr][i] - (scale ? tminV : minV)) * (yy-frame) / delta); 101 | else buf[i] = 0; 102 | } 103 | 104 | for (int i = res; i < size+1; i++) { 105 | if (count > 0) plot.line((i-1) * res, yy - buf[(i + count - 1) % size], i * res, yy - buf[(i + count) % size]); 106 | } 107 | 108 | plot.endDraw(); 109 | } 110 | 111 | // отобразить 112 | void update() { 113 | image(canvas, x, y); 114 | image(plot, x, y); 115 | plot.beginDraw(); 116 | plot.background(0, 0); 117 | plot.stroke(0, 100); 118 | plot.fill(0); 119 | for (int i = 0; i < lnum; i++) { 120 | int posY = (i + 1) * yy / (lnum + 1); 121 | plot.line(frame, posY, xx, posY); 122 | if (delta != 0) plot.text((yy-posY) * delta / yy + (scale ? tminV : minV), 10, posY - 2); 123 | } 124 | if (minmax) { 125 | plot.text("max: " + tmaxV, xx/2 - 15, 14); 126 | plot.text("min: " + tminV, xx/2 - 15, yy-2); 127 | } 128 | plot.endDraw(); 129 | count++; 130 | } 131 | 132 | // переменные 133 | int x, xx, y, yy; 134 | int lnum = 0; 135 | int count, res, axes, size; 136 | int buffer[][]; 137 | int minV, maxV; 138 | int tminV, tmaxV; 139 | int delta; 140 | boolean scale = true, minmax = false; 141 | PGraphics plot, canvas; 142 | 143 | int frame = 2; 144 | int hueStep = 151; // шаг цвета 145 | }; 146 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/stepper_cl.pde: -------------------------------------------------------------------------------- 1 | class Stepper { 2 | Stepper() { 3 | } 4 | 5 | void set(int newPos) { 6 | pos = newPos; 7 | } 8 | 9 | void setTarget(int ntar, int t) { 10 | tar = ntar; 11 | dir = (pos < tar) ? 1 : -1; 12 | dt = (pos == tar) ? 0 : (t / abs(pos - tar)); 13 | tmr = millis(); 14 | startF = true; 15 | } 16 | 17 | void stepTo(int tar) { 18 | if (tar > pos) pos++; 19 | else if (tar < pos) pos--; 20 | } 21 | 22 | void step(int st) { 23 | pos += st; 24 | } 25 | 26 | void setPos(int npos) { 27 | pos = npos; 28 | } 29 | 30 | void start() { 31 | startF = true; 32 | } 33 | 34 | void stop() { 35 | startF = false; 36 | } 37 | 38 | boolean tick() { 39 | if (!startF) return false; 40 | if (pos == tar || dt == 0) return true; 41 | if (millis() - tmr >= dt) { 42 | tmr += dt; 43 | pos += dir; 44 | } 45 | return false; 46 | } 47 | 48 | int tmr, dt; 49 | int pos = 0, tar = 0, dir = 1; 50 | boolean startF = false; 51 | }; 52 | -------------------------------------------------------------------------------- /Planner Simulation/Planner/timer_cl.pde: -------------------------------------------------------------------------------- 1 | class Timer { 2 | Timer(int newDt) { 3 | dt = newDt; 4 | } 5 | 6 | void start() { 7 | startF = true; 8 | } 9 | 10 | void stop() { 11 | startF = false; 12 | } 13 | 14 | boolean tick() { 15 | if (startF && millis() - tmr >= dt) { 16 | tmr = millis(); 17 | return true; 18 | } 19 | return false; 20 | } 21 | 22 | int dt = 0, tmr = 0; 23 | boolean startF = false; 24 | }; 25 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/Planner2.pde: -------------------------------------------------------------------------------- 1 | // СИМУЛЯЦИЯ ПЛАНИРОВЩИКА GPLANNER2 2 | 3 | int type = 2; // тип маршрута: 0 - ручной массив, 1 - случайный, 2 - окружность 4 | int dots = 20; // количество точек в маршруте 5 | 6 | int mul = 1; // уменьшить разрешение по шагам (например 6) 7 | boolean show = false; // показать сетку текущего разрешения 8 | 9 | // массив маршрута тип 0. Третья координата - 1/0 точка остановки 10 | int points[][] = { 11 | {100, 20, 0}, 12 | {150, 20, 0}, 13 | {200, 20, 0}, 14 | {300, 20, 0}, 15 | {300, 200, 0}, 16 | {310, 20, 0}, 17 | {350, 20, 0}, 18 | {500, 200, 0}, 19 | }; 20 | 21 | // 22 | int WX = 800; 23 | int HX = 800; 24 | 25 | Planner_v4 pln = new Planner_v4(2); 26 | 27 | Plotter plotterX = new Plotter(0, HX/2, WX/2-10, HX/2-10, 2, 2); 28 | Plotter plotterY = new Plotter(WX/2, HX/2, WX/2-10, HX/2-10, 2, 2); 29 | 30 | Timer timer = new Timer(10); 31 | 32 | MyBtn start = new MyBtn(false, 10, 10, 80, 20, "start"); 33 | MyBtn resume = new MyBtn(false, 10, 30, 80, 20, "resume"); 34 | MyBtn stop = new MyBtn(false, 10, 50, 80, 20, "stop"); 35 | MyBtn brake = new MyBtn(false, 10, 70, 80, 20, "brake"); 36 | 37 | void settings() { 38 | size(WX, HX); 39 | smooth(8); 40 | } 41 | 42 | void setup() { 43 | frameRate(1000); 44 | colorMode(HSB); 45 | background(210); 46 | randomSeed(6); 47 | 48 | // забиваем маршрут 49 | if (type == 1) { 50 | points = new int[dots][4]; 51 | for (int i = 0; i < dots; i++) { 52 | points[i][0] = int(random(0, width)); 53 | points[i][1] = int(random(0, height / 2)); 54 | } 55 | } else if (type == 2) { 56 | points = new int[dots][4]; 57 | for (int i = 0; i < dots; i++) { 58 | float radius = 180; 59 | points[i][0] = width/2+int(radius*cos((float)i/dots*2*PI)); 60 | points[i][1] = height/4+int(radius*sin((float)i/dots*2*PI)); 61 | } 62 | } 63 | 64 | // на последней точке остановимся 65 | points[points.length-1][2] = 1; 66 | 67 | // даунскейлинг если надо 68 | for (int i = 0; i < points.length; i++) { 69 | points[i][0] /= mul; 70 | points[i][1] /= mul; 71 | } 72 | 73 | // начальные условия 74 | pln.setCurrent(points[0]); 75 | pln.setMaxSpeed(300); 76 | pln.setAcceleration(400); 77 | 78 | // запускаем плотер 79 | plotterX.init(); 80 | plotterY.init(); 81 | plotterX.showMinMax(true); 82 | plotterY.showMinMax(true); 83 | plotterY.setLineAmount(10); 84 | plotterX.setLineAmount(10); 85 | plotterX.autoScale(false); // выключим авто масштаб (по умолч. включен) 86 | plotterX.setMin(0); // минимум для фикс масштаба 87 | plotterX.setMax(350); // максимум для фикс масштаба 88 | plotterY.autoScale(false); // выключим авто масштаб (по умолч. включен) 89 | plotterY.setMin(0); // минимум для фикс масштаба 90 | plotterY.setMax(350); // максимум для фикс масштаба 91 | timer.start(); 92 | 93 | // рисуем путь 94 | drawPath(show); 95 | } 96 | 97 | int count = 0; 98 | void draw() { 99 | // планировщик 100 | pln.tick(); 101 | if (pln.available()) { 102 | pln.addPoint(points[count], points[count][2]); 103 | if (++count >= points.length) count = 0; 104 | } 105 | 106 | // кнопки 107 | if (start.pool()) pln.start(); 108 | if (stop.pool()) pln.stop(); 109 | if (resume.pool()) pln.resume(); 110 | if (brake.pool()) pln.brake(); 111 | 112 | // плоттер 113 | if (timer.tick()) { 114 | plotterX.add(0, pln.steppers[0].pos / 5); 115 | plotterX.add(1, pln.us != 0 ? int(1000000/pln.us) : 0); 116 | plotterX.update(); 117 | 118 | plotterY.add(0, pln.steppers[1].pos / 5); 119 | plotterY.add(1, pln.us != 0 ? int(1000000/pln.us) : 0); 120 | plotterY.update(); 121 | } 122 | 123 | // статус планировщика 124 | fill(210); 125 | rect(10, 380-15, 150, 30); 126 | fill(0); 127 | String str = ""; 128 | switch (pln.status) { 129 | case 0: 130 | str = "wait"; 131 | break; 132 | case 1: 133 | str = "buffer"; 134 | break; 135 | case 2: 136 | str = "run"; 137 | break; 138 | case 3: 139 | str = "to pause"; 140 | break; 141 | case 4: 142 | str = "to stop"; 143 | break; 144 | case 5: 145 | str = "speed"; 146 | break; 147 | } 148 | text(str, 10, 380); 149 | } 150 | 151 | // отрисовать маршрут 152 | void drawPath(boolean grid) { 153 | stroke(150); 154 | if (grid) { 155 | for (int i = 0; i < 200; i++) { 156 | line(i*mul, 0, i*mul, height); 157 | line(0, i*mul, width, i*mul); 158 | } 159 | } 160 | for (int i = 0; i < points.length; i++) { 161 | fill(0); 162 | noStroke(); 163 | circle(points[i][0]*mul, points[i][1]*mul, 8); 164 | stroke(0); 165 | strokeWeight(0.5); 166 | if (i > 0) line(points[i-1][0]*mul, points[i-1][1]*mul, points[i][0]*mul, points[i][1]*mul); 167 | text(i, points[i][0]*mul+10, points[i][1]*mul); 168 | //text(points[i][3], points[i][0]*mul+10, points[i][1]*mul+12); 169 | } 170 | noStroke(); 171 | } 172 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/button_cl.pde: -------------------------------------------------------------------------------- 1 | // type 0 - momentary, type 1 - fixed 2 | class MyBtn { 3 | MyBtn(boolean type, int x, int y, int a, int b, String text) { 4 | _type = type; 5 | _x = x; 6 | _y = y; 7 | _a = a; 8 | _b = b; 9 | _c1 = 180; 10 | _c2 = 150; 11 | _c3 = 120; 12 | _c4 = 0; 13 | _text = text; 14 | } 15 | MyBtn(boolean type, int x, int y, int a, int b, color c1, color c2, color c3, String text, color c4) { 16 | _type = type; 17 | _x = x; 18 | _y = y; 19 | _a = a; 20 | _b = b; 21 | _c1 = c1; 22 | _c2 = c2; 23 | _c3 = c3; 24 | _c4 = c4; 25 | _text = text; 26 | } 27 | boolean pool() { 28 | boolean flag = false; 29 | if ((mouseX > _x) && (mouseX < (_x+_a)) && (mouseY > _y) && (mouseY < (_y+_b))) { 30 | if (mousePressed) { 31 | if (!_flag) { 32 | if (_type) _state = !_state; 33 | else { 34 | flag = _state = true; 35 | } 36 | _flag = true; 37 | } 38 | fill(_c3); 39 | } else { 40 | if (_flag) _flag = false; 41 | fill(_c2); 42 | } 43 | } else { 44 | fill(_c1); 45 | if (_flag) _flag = false; 46 | } 47 | if (_state && _type) fill(_c3); 48 | stroke(0); 49 | strokeWeight(1); 50 | rect(_x, _y, _a, _b); 51 | noStroke(); 52 | fill(_c4); 53 | textSize(15); 54 | text(_text, _x+5, _y+15); 55 | return flag; 56 | } 57 | boolean check() { 58 | if (_type) return _state; 59 | else { 60 | if (_state) { 61 | _state = false; 62 | return true; 63 | } 64 | } 65 | return false; 66 | } 67 | int _x, _y, _a, _b; 68 | color _c1, _c2, _c3, _c4; 69 | String _text; 70 | boolean _state = false, _flag = false, _type = false; 71 | }; 72 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/fifo_cl.pde: -------------------------------------------------------------------------------- 1 | class FIFO { 2 | FIFO(int nsize) { 3 | size = nsize; 4 | clear(); 5 | buf = new int[size]; 6 | for (int i = 0; i < size; i++) buf[i] = 0; 7 | } 8 | 9 | void clear() { 10 | head = tail = 0; 11 | } 12 | 13 | int available() { 14 | return (size + head - tail) % size; 15 | } 16 | 17 | int get(int i) { 18 | return buf[(tail + i) % size]; 19 | } 20 | 21 | void add(int p) { 22 | int i = (head + 1) % size; 23 | if (i != tail) { 24 | buf[head] = p; 25 | head = i; 26 | } 27 | } 28 | 29 | void set(int i, int p) { 30 | buf[(tail + i) % size] = p; 31 | } 32 | 33 | void next() { 34 | buf[tail] = 0; 35 | tail = (tail + 1) % size; 36 | } 37 | 38 | boolean availableForWrite() { 39 | return ((head + 1) % size != tail); 40 | } 41 | 42 | int buf[]; 43 | int size, head, tail; 44 | }; 45 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/planner_v4.pde: -------------------------------------------------------------------------------- 1 | class Planner_v4 { 2 | Planner_v4(int n_AXLES) { 3 | _AXLES = n_AXLES; 4 | steppers = new Stepper[_AXLES]; 5 | for (int i = 0; i < _AXLES; i++) steppers[i] = new Stepper(); 6 | bufP = new FIFO[_AXLES]; 7 | for (int i = 0; i < _AXLES; i++) bufP[i] = new FIFO(_BUF); 8 | dS = new int[2]; 9 | nd = new int[2]; 10 | } 11 | 12 | void setDtA(float newDta) { 13 | dtA = newDta; 14 | } 15 | 16 | void setMaxSpeed(float newV) { 17 | V = newV; 18 | usMin = int(1000000.0 / V); 19 | } 20 | 21 | void setAcceleration(int newA) { 22 | a = newA; 23 | if (a != 0) us0 = int(0.676 * 1000000 * sqrt(2.0 / a)); 24 | else us0 = 0; 25 | } 26 | 27 | void stop() { 28 | if (us == 0 || status == 0 || status == 4) return; // мы и так уже остановились, успокойся 29 | if (a == 0) { // нет ускорения - дёргай ручник 30 | brake(); 31 | return; 32 | } 33 | stopStep = 1000000 / us; // наша скорость 34 | stopStep = stopStep * stopStep / (2 * a); // дистанция остановки 35 | us8 = us << 8; // период 36 | status = 4; // статус на стоп 37 | } 38 | 39 | void start() { 40 | if (status == 0) status = 1; // если остановлен - проверяем буфер 41 | } 42 | 43 | void brake() { 44 | status = 0; 45 | us = 0; 46 | // пишем в буфер что мы остановились 47 | for (int i = 0; i < _AXLES; i++) bufP[i].set(0, steppers[i].pos); 48 | bufV.set(0, 0); 49 | } 50 | 51 | void resume() { 52 | if (status == 0) status = 1; // если стоим ждём - проверить буфер 53 | } 54 | 55 | void setCurrent(int cur[]) { 56 | for (int i = 0; i < _AXLES; i++) steppers[i].pos = cur[i]; 57 | } 58 | 59 | void setTarget() { 60 | tmr = millis(); 61 | for (int i = 0; i < _AXLES; i++) { // для всех осей 62 | dS[i] = abs(bufP[i].get(1) - steppers[i].pos); // модуль ошибки по оси 63 | steppers[i].dir = steppers[i].pos < bufP[i].get(1) ? 1 : -1; // направление движения по оси 64 | } 65 | S = bufS.get(0); 66 | for (int i = 0; i < _AXLES; i++) nd[i] = S / 2; // записываем половину 67 | 68 | if (S == 0) { // путь == 0, мы никуда не едем 69 | status = 1; // на буфер 70 | next(); 71 | return; 72 | } 73 | 74 | if (a > 0) { 75 | int v1 = bufV.get(0); // скорость начала отрезка 76 | int v2 = bufV.get(1); 77 | 78 | if (2.0*V*V-v1*v1-v2*v2 > 2.0*a*S) { // треугольник 79 | s1 = int((2.0*a*S + v2*v2 - v1*v1) / (4.0*a)); 80 | s2 = 0; 81 | } else { // трапеция 82 | s1 = int((V*V - v1*v1) / (2 * a)); 83 | s2 = int(S - (V*V - v2*v2) / (2 * a)); 84 | } 85 | so1 = v1 * v1 / (2 * a); 86 | so2 = v2 * v2 / (2 * a); 87 | if (status != 4) { 88 | if (v1 == 0) us = us0; 89 | else us = 1000000 / v1; 90 | } 91 | } else { 92 | so1 = so2 = 0; 93 | s1 = 0; 94 | s2 = S; 95 | us = usMin; 96 | } 97 | 98 | step = 0; 99 | readyF = false; 100 | if (status != 4) { // если это не стоп 101 | if (bufL.get(1) == 1) status = 3; 102 | else status = 2; 103 | us8 = us << 8; 104 | } 105 | } 106 | 107 | // вернёт false если мотор приехал 108 | boolean tick() { 109 | checkBuffer(); 110 | if (status > 1 && millis() - tmr >= us/1000) { 111 | tmr = millis(); 112 | tickManual(); 113 | 114 | // ВЫВОД МАРШРУТА 115 | int vel = us==0 ? 0 : 1000000/us; 116 | int hue = int(map(vel, 0, V, 0, 70)); 117 | fill(hue, 255, 255); 118 | circle(steppers[0].pos*mul, steppers[1].pos*mul, 4); 119 | // ВЫВОД МАРШРУТА 120 | } 121 | return status > 1; 122 | } 123 | 124 | boolean tickManual() { 125 | if (status == 5) { 126 | return true; 127 | } 128 | 129 | step++; 130 | for (int i = 0; i < _AXLES; i++) { 131 | // http://members.chello.at/easyfilter/bresenham.html 132 | nd[i] -= dS[i]; 133 | if (nd[i] < 0) { 134 | nd[i] += S; 135 | steppers[i].step(); // двигаем мотор в направлении sn 136 | } 137 | } 138 | 139 | if (status == 4) { 140 | stopStep--; 141 | us8 += 2 * us8 / (4 * stopStep + 1); // торможение 142 | us = us8 >> 8; 143 | us = constrain(us, usMin, us0); 144 | if (step >= S) { 145 | next(); 146 | setTarget(); 147 | us = us8 >> 8; 148 | } 149 | if (stopStep <= 0 || us >= us0) { 150 | brake(); 151 | } 152 | return true; 153 | } 154 | 155 | if (step < s1) { // разгон 156 | us8 -= 2 * us8 / (4 * (step + so1) + 1); 157 | us = us8 >> 8; 158 | us = constrain(us, usMin, us0); 159 | } else if (step < s2) us = usMin; // постоянная 160 | else if (step < S) { // торможение 161 | us8 += 2 * us8 / (4 * (S + so2 - step) + 1); 162 | us = us8 >> 8; 163 | us = constrain(us, usMin, us0); 164 | } else { // приехали 165 | if (status == 3) { // достигли конечной точки 166 | readyF = true; 167 | status = 0; 168 | } else status = 1; // иначе проверяем буфер 169 | next(); 170 | } 171 | return status > 1; 172 | } 173 | 174 | void checkBuffer() { 175 | if (status == 1) { 176 | if (bufV.available() > 1) { 177 | if (bufV.get(0) == 0) calculateBlock(); 178 | setTarget(); 179 | } 180 | } 181 | } 182 | 183 | void next() { 184 | for (int i = 0; i < _AXLES; i++) bufP[i].next(); 185 | bufL.next(); 186 | bufV.next(); 187 | bufS.next(); 188 | } 189 | 190 | boolean ready() { 191 | if (readyF) { 192 | readyF = false; 193 | return true; 194 | } 195 | return false; 196 | } 197 | 198 | boolean available() { 199 | return bufV.availableForWrite(); 200 | } 201 | 202 | void addPoint(int[] tar, int l) { 203 | for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i]); 204 | bufL.add(l); 205 | bufV.add(0); 206 | bufS.add(0); 207 | } 208 | 209 | void calculateBlock() { 210 | // поиск максимальной конечной скорости 211 | for (int i = 0; i < bufV.available() - 1; i++) { 212 | int sqSum = 0; 213 | int[] dn = new int[_AXLES]; 214 | int[] dn1 = new int[_AXLES]; 215 | for (int j = 0; j < _AXLES; j++) { 216 | dn[j] = bufP[j].get(i+1) - bufP[j].get(i); 217 | sqSum += dn[j] * dn[j]; 218 | } 219 | int s1 = int(sqrt(sqSum)); 220 | bufS.set(i, s1); 221 | 222 | if (bufL.get(i + 1) == 1) break; 223 | if (a == 0) { 224 | bufV.set(i + 1, int(V)); 225 | continue; 226 | } 227 | 228 | if (i < bufV.available() - 2) { 229 | int multSum = 0; 230 | sqSum = 0; 231 | for (int j = 0; j < _AXLES; j++) { 232 | dn1[j] = bufP[j].get(i + 2) - bufP[j].get(i + 1); 233 | sqSum += dn1[j] * dn1[j]; 234 | multSum += dn[j] * dn1[j]; 235 | } 236 | int s2 = int(sqrt(sqSum)); 237 | if (s1 == 0 || s2 == 0) continue; 238 | float cosa = -(float)multSum / (s1 * s2); 239 | float v2; 240 | if (cosa < -0.9) v2 = V; 241 | else v2 = a * dtA / sqrt(2.0 * (1 + cosa)); 242 | v2 = min(v2, V); 243 | bufV.set(i + 1, int(v2)); 244 | } 245 | } 246 | 247 | // рекурсивно уменьшаем конечные скорости на участке 248 | for (int i = 0; i < bufV.available() - 1; i++) { 249 | int v0 = bufV.get(i); 250 | int v1 = bufV.get(i + 1); 251 | int maxV = int(sqrt(2.0 * a * bufS.get(i) + v0 * v0)); 252 | if (v1 > v0 && maxV < v1) { 253 | bufV.set(i + 1, maxV); 254 | } else if (v1 < v0 && maxV > v1) { 255 | int count = 0; 256 | while (true) { 257 | int minV = int(sqrt(2.0 * a * bufS.get(i + count) + bufV.get(i + count + 1) * bufV.get(i + count + 1))); 258 | if (minV >= bufV.get(i + count)) break; 259 | else bufV.set(i + count, minV); 260 | count--; 261 | } 262 | } 263 | } 264 | } 265 | 266 | // status 267 | // 0 ожидание команды (остановлен) 268 | // 1 ожидание буфера 269 | // 2 в пути 270 | // 3 на паузу 271 | // 4 на стоп 272 | // 5 крутится 273 | 274 | int[] nd; 275 | int S, a; 276 | float V; 277 | int s1, s2, so1, so2; 278 | int[] dS; 279 | int us0, us = 0, us8, tmr, usMin, step; 280 | int stopStep; 281 | byte status = 0; 282 | boolean readyF = false; 283 | Stepper[] steppers; 284 | 285 | float dtA = 0.3; 286 | 287 | int _BUF = 10; 288 | int _AXLES = 0; 289 | FIFO[] bufP; 290 | FIFO bufL = new FIFO(_BUF); 291 | FIFO bufV = new FIFO(_BUF); 292 | FIFO bufS = new FIFO(_BUF); 293 | }; 294 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/plotter_cl.pde: -------------------------------------------------------------------------------- 1 | // простая библиотек плоттера для Processing 2 | // v1.0 - релиз 3 | 4 | class Plotter { 5 | // x, y, size x, size y, axes, resolution 6 | Plotter(int nx, int ny, int nxx, int nyy, int naxes, int nres) { 7 | x = nx; 8 | xx = nxx; 9 | y = ny; 10 | yy = nyy; 11 | res = nres; 12 | axes = naxes; 13 | size = xx / nres; 14 | buffer = new int[axes][size]; 15 | } 16 | 17 | // автомасштаб 18 | void autoScale(boolean nscale) { 19 | scale = nscale; 20 | } 21 | 22 | // минимум (для фикс масштаба) 23 | void setMin(int nminV) { 24 | minV = nminV; 25 | } 26 | 27 | // максимум (для фикс масштаба) 28 | void setMax(int nmaxV) { 29 | maxV = nmaxV; 30 | } 31 | 32 | // кол-во делений 33 | void setLineAmount(int nlnum) { 34 | lnum = nlnum; 35 | } 36 | 37 | // отображение минимума и максимума 38 | void showMinMax(boolean nminmax) { 39 | minmax = nminmax; 40 | } 41 | 42 | // инициализация 43 | void init() { 44 | plot = createGraphics(xx+frame*2, yy+frame*2); 45 | plot.beginDraw(); 46 | plot.textSize(11); 47 | plot.colorMode(HSB, 255, 255, 255); 48 | plot.background(0, 0); 49 | plot.endDraw(); 50 | canvas = createGraphics(xx+frame*2, yy+frame*2); 51 | canvas.beginDraw(); 52 | canvas.colorMode(HSB, 255, 255, 255); 53 | canvas.fill(255); 54 | canvas.stroke(0); 55 | canvas.strokeWeight(frame); 56 | canvas.rect(frame, frame, xx, yy); 57 | canvas.strokeWeight(1); 58 | for (int i = 0; i < axes; i++) { 59 | canvas.fill((i * hueStep) % 255, 255, 200); 60 | canvas.rect(8 + i * 16, 8, 12, 12); 61 | } 62 | canvas.endDraw(); 63 | image(canvas, x, y); 64 | } 65 | 66 | // добавить точку (ось, значение) 67 | void add(int addr, float val) { 68 | add(addr, int(val)); 69 | } 70 | 71 | // добавить точку (ось, значение) 72 | void add(int addr, int val) { 73 | plot.beginDraw(); 74 | plot.strokeWeight(1.2); 75 | plot.stroke((addr * hueStep) % 255, 255, 200); 76 | buffer[addr][count % size] = val; 77 | 78 | tmaxV = maxV; 79 | tminV = minV; 80 | int buf[] = new int [size]; 81 | 82 | tmaxV = -0xFFFFFF; 83 | tminV = 0xFFFFFF; 84 | for (int a = 0; a < axes; a++) { 85 | for (int i = 0; i < size; i++) { 86 | tmaxV = max(tmaxV, buffer[a][i]); 87 | tminV = min(tminV, buffer[a][i]); 88 | } 89 | } 90 | 91 | if (!scale) { 92 | for (int i = 0; i < size; i++) { 93 | buf[i] = constrain(buffer[addr][i], minV, maxV); 94 | } 95 | } 96 | 97 | if (scale) delta = abs(tmaxV) + abs(tminV); 98 | else delta = abs(maxV) + abs(minV); 99 | for (int i = 0; i < size; i++) { 100 | if (delta != 0) buf[i] = int((buffer[addr][i] - (scale ? tminV : minV)) * (yy-frame) / delta); 101 | else buf[i] = 0; 102 | } 103 | 104 | for (int i = res; i < size+1; i++) { 105 | if (count > 0) plot.line((i-1) * res, yy - buf[(i + count - 1) % size], i * res, yy - buf[(i + count) % size]); 106 | } 107 | 108 | plot.endDraw(); 109 | } 110 | 111 | // отобразить 112 | void update() { 113 | image(canvas, x, y); 114 | image(plot, x, y); 115 | plot.beginDraw(); 116 | plot.background(0, 0); 117 | plot.stroke(0, 100); 118 | plot.fill(0); 119 | for (int i = 0; i < lnum; i++) { 120 | int posY = (i + 1) * yy / (lnum + 1); 121 | plot.line(frame, posY, xx, posY); 122 | if (delta != 0) plot.text((yy-posY) * delta / yy + (scale ? tminV : minV), 10, posY - 2); 123 | } 124 | if (minmax) { 125 | plot.text("max: " + tmaxV, xx/2 - 15, 14); 126 | plot.text("min: " + tminV, xx/2 - 15, yy-2); 127 | } 128 | plot.endDraw(); 129 | count++; 130 | } 131 | 132 | // переменные 133 | int x, xx, y, yy; 134 | int lnum = 0; 135 | int count, res, axes, size; 136 | int buffer[][]; 137 | int minV, maxV; 138 | int tminV, tmaxV; 139 | int delta; 140 | boolean scale = true, minmax = false; 141 | PGraphics plot, canvas; 142 | 143 | int frame = 2; 144 | int hueStep = 151; // шаг цвета 145 | }; 146 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/stepper_cl.pde: -------------------------------------------------------------------------------- 1 | class Stepper { 2 | Stepper() { 3 | } 4 | 5 | void set(int newPos) { 6 | pos = newPos; 7 | } 8 | 9 | void setTarget(int ntar, int t) { 10 | tar = ntar; 11 | dir = (pos < tar) ? 1 : -1; 12 | dt = (pos == tar) ? 0 : (t / abs(pos - tar)); 13 | tmr = millis(); 14 | startF = true; 15 | } 16 | 17 | void stepTo(int tar) { 18 | if (tar > pos) pos++; 19 | else if (tar < pos) pos--; 20 | } 21 | 22 | void step() { 23 | pos += dir; 24 | } 25 | 26 | void setPos(int npos) { 27 | pos = npos; 28 | } 29 | 30 | void start() { 31 | startF = true; 32 | } 33 | 34 | void stop() { 35 | startF = false; 36 | } 37 | 38 | boolean tick() { 39 | if (!startF) return false; 40 | if (pos == tar || dt == 0) return true; 41 | if (millis() - tmr >= dt) { 42 | tmr += dt; 43 | pos += dir; 44 | } 45 | return false; 46 | } 47 | 48 | int tmr, dt; 49 | int pos = 0, tar = 0, dir = 1; 50 | boolean startF = false; 51 | }; 52 | -------------------------------------------------------------------------------- /Planner Simulation/Planner2/timer_cl.pde: -------------------------------------------------------------------------------- 1 | class Timer { 2 | Timer(int newDt) { 3 | dt = newDt; 4 | } 5 | 6 | void start() { 7 | startF = true; 8 | } 9 | 10 | void stop() { 11 | startF = false; 12 | } 13 | 14 | boolean tick() { 15 | if (startF && millis() - tmr >= dt) { 16 | tmr = millis(); 17 | return true; 18 | } 19 | return false; 20 | } 21 | 22 | int dt = 0, tmr = 0; 23 | boolean startF = false; 24 | }; 25 | -------------------------------------------------------------------------------- /docs/a4988.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GyverLibs/GyverStepper/4616e9a809a4882963b54edaf66e784e7e6260c1/docs/a4988.jpg -------------------------------------------------------------------------------- /docs/drv8825_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GyverLibs/GyverStepper/4616e9a809a4882963b54edaf66e784e7e6260c1/docs/drv8825_2.jpg -------------------------------------------------------------------------------- /docs/tb6560.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GyverLibs/GyverStepper/4616e9a809a4882963b54edaf66e784e7e6260c1/docs/tb6560.jpg -------------------------------------------------------------------------------- /docs/tmc2208.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GyverLibs/GyverStepper/4616e9a809a4882963b54edaf66e784e7e6260c1/docs/tmc2208.jpg -------------------------------------------------------------------------------- /docs/uln2003.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GyverLibs/GyverStepper/4616e9a809a4882963b54edaf66e784e7e6260c1/docs/uln2003.jpg -------------------------------------------------------------------------------- /examples/Planner/PlannerArray/PlannerArray.ino: -------------------------------------------------------------------------------- 1 | // пример с записанным в памяти маршрутом 2 | // смотри график 3 | 4 | int path[][2] = { 5 | {10, 10}, 6 | {100, 5}, 7 | {200, 200}, 8 | {150, 150}, 9 | {0, 0}, 10 | {0, 50}, 11 | }; 12 | 13 | // количество точек (пусть компилятор сам считает) 14 | // как вес всего массива / (2+2) байта 15 | int nodeAmount = sizeof(path) / 4; 16 | 17 | #include "GyverPlanner.h" 18 | Stepper stepper1(2, 5); 19 | Stepper stepper2(3, 6); 20 | GPlanner planner; 21 | 22 | void setup() { 23 | Serial.begin(115200); 24 | // добавляем шаговики на оси 25 | planner.addStepper(0, stepper1); // ось 0 26 | planner.addStepper(1, stepper2); // ось 1 27 | 28 | // устанавливаем ускорение и скорость 29 | planner.setAcceleration(100); 30 | planner.setMaxSpeed(300); 31 | } 32 | 33 | int count = 0; // счётчик точек маршрута 34 | void loop() { 35 | // здесь происходит движение моторов, вызывать как можно чаще 36 | planner.tick(); 37 | 38 | // вернёт true, если все моторы доехали 39 | if (planner.ready()) { 40 | 41 | if (count < nodeAmount) { // ограничиваем на количество точек, чтобы не бахнуло 42 | planner.setTarget(path[count]); // загружаем новую точку (начнётся с 0) 43 | count++; 44 | } 45 | } 46 | 47 | // асинхронно вывожу в порт графики 48 | static uint32_t tmr; 49 | if (millis() - tmr >= 20) { 50 | tmr = millis(); 51 | Serial.print(stepper1.pos); 52 | Serial.print(','); 53 | Serial.println(stepper2.pos); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /examples/Planner/PlannerCircle/PlannerCircle.ino: -------------------------------------------------------------------------------- 1 | // пример с записанным в памяти маршрутом окружности 2 | // работу можно посмотреть в плоттере, а лучше в приложенном stepperPlot 3 | 4 | const int pointAm = 30; // количество точек в круге 5 | int radius = 100; // радиус круга 6 | int32_t path[pointAm + 2][2]; // буфер круга 7 | 8 | #include "GyverPlanner.h" 9 | Stepper stepper1(2, 5); 10 | Stepper stepper2(3, 6); 11 | GPlanner planner; 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | // добавляем шаговики на оси 16 | planner.addStepper(0, stepper1); // ось 0 17 | planner.addStepper(1, stepper2); // ось 1 18 | 19 | // устанавливаем ускорение и скорость 20 | planner.setAcceleration(500); 21 | planner.setMaxSpeed(500); 22 | 23 | // заполняем буфер 24 | for (int i = 0; i <= pointAm; i++) { 25 | path[i + 1][0] = radius + radius * cos(TWO_PI * i / pointAm); 26 | path[i + 1][1] = radius + radius * sin(TWO_PI * i / pointAm); 27 | } 28 | } 29 | 30 | int count = 0; // счётчик точек маршрута 31 | void loop() { 32 | // здесь происходит движение моторов, вызывать как можно чаще 33 | planner.tick(); 34 | 35 | // вернёт true, если все моторы доехали 36 | if (planner.ready()) { 37 | planner.setTarget(path[count]); // загружаем новую точку (начнётся с 0) 38 | if (++count >= sizeof(path) / 8) count = 0; 39 | } 40 | 41 | // асинхронно вывожу в порт графики 42 | static uint32_t tmr; 43 | if (millis() - tmr >= 20) { 44 | tmr = millis(); 45 | Serial.print(stepper1.pos); 46 | Serial.print(','); 47 | Serial.println(stepper2.pos); 48 | } 49 | } -------------------------------------------------------------------------------- /examples/Planner/PlannerCircle/stepperPlot/stepperPlot.pde: -------------------------------------------------------------------------------- 1 | String port = "COM6"; // имя порта 2 | 3 | import processing.serial.*; 4 | Serial myPort; 5 | 6 | void setup() { 7 | size(500, 500); 8 | myPort = new Serial(this, port, 115200); 9 | fill(0); 10 | noStroke(); 11 | } 12 | 13 | void draw() { 14 | if (myPort.available() > 0) { 15 | String str = myPort.readStringUntil('\n').trim(); 16 | if (str != null) { 17 | String[] pos = split(str, ','); 18 | int x = int(pos[0]); 19 | int y = int(pos[1]); 20 | fill(10); 21 | circle(x+10, y+10, 4); 22 | fill(200, 1); 23 | rect(0, 0, width, height); 24 | } 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /examples/Planner/PlannerControl/PlannerControl.ino: -------------------------------------------------------------------------------- 1 | // здесь у нас моторы движутся по трём точкам траектории 2 | // можно открыть плоттер, наблюать за этим и отправлять команды: 3 | // s - стоп 4 | // b - тормоз 5 | // p - пауза 6 | // r - продолжить 7 | 8 | #include "GyverPlanner.h" 9 | Stepper stepper1(2, 5); 10 | Stepper stepper2(3, 6); 11 | GPlanner planner; 12 | 13 | void setup() { 14 | Serial.begin(57600); 15 | // добавляем шаговики на оси 16 | planner.addStepper(0, stepper1); // ось 0 17 | planner.addStepper(1, stepper2); // ось 1 18 | 19 | // устанавливаем ускорение и скорость 20 | planner.setAcceleration(200); 21 | planner.setMaxSpeed(200); 22 | } 23 | 24 | byte count = 0; 25 | int32_t path[][2] = { 26 | {0, 0}, 27 | {100, 150}, 28 | {200, 200}, 29 | }; 30 | 31 | void loop() { 32 | // здесь происходит движение моторов, вызывать как можно чаще 33 | planner.tick(); 34 | 35 | // вернёт true, если все моторы доехали 36 | if (planner.ready()) { 37 | planner.setTarget(path[count]); // загружаем новую точку (начнётся с 0) 38 | if (++count >= sizeof(path) / 8) count = 0; 39 | } 40 | 41 | // управляем процессом 42 | if (Serial.available() > 0) { 43 | char incoming = Serial.read(); 44 | switch (incoming) { 45 | case 's': planner.stop(); break; 46 | case 'b': planner.brake(); break; 47 | case 'r': planner.resume(); break; 48 | case 'p': planner.pause(); break; 49 | } 50 | } 51 | 52 | // асинхронно вывожу в порт графики 53 | static uint32_t tmr; 54 | if (millis() - tmr >= 20) { 55 | tmr = millis(); 56 | Serial.print(planner.getTarget(0)); 57 | Serial.print(','); 58 | Serial.print(planner.getTarget(1)); 59 | Serial.print(','); 60 | Serial.print(stepper1.pos); 61 | Serial.print(','); 62 | Serial.println(stepper2.pos); 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /examples/Planner/PlannerDemo/PlannerDemo.ino: -------------------------------------------------------------------------------- 1 | // базовый пример: как создать и запустить планировщик 2 | // при запуске моторы будут отправлены на первую позицию 3 | // при достижении - на вторую. После этого движение прекратится 4 | // открой плоттер и смотри графики 5 | 6 | #include "GyverPlanner.h" 7 | // создаём моторы класса Stepper с указанием типа драйвера и пинов. Примеры: 8 | // Stepper stepper(step, dir); // драйвер step-dir 9 | // Stepper stepper(step, dir, en); // драйвер step-dir + пин enable 10 | // Stepper stepper(pin1, pin2, pin3, pin4); // драйвер 4 пин 11 | // Stepper stepper(pin1, pin2, pin3, pin4, en); // драйвер 4 пин + enable 12 | // Stepper stepper(pin1, pin2, pin3, pin4); // драйвер 4 пин полушаг 13 | // Stepper stepper(pin1, pin2, pin3, pin4, en); // драйвер 4 пин полушаг + enable 14 | 15 | // МОТОРЫ ДОЛЖНЫ БЫТЬ С ОДИНАКОВЫМ ТИПОМ ДРАЙВЕРА 16 | // вот они красавцы 17 | Stepper stepper1(2, 5); 18 | Stepper stepper2(3, 6); 19 | 20 | // создаём планировщик, указываем в <> тип драйвера КАК У МОТОРОВ 21 | // и количество осей, равное количеству моторов (любое больше 1) 22 | GPlanner planner; 23 | 24 | void setup() { 25 | Serial.begin(115200); 26 | // добавляем шаговики на оси 27 | planner.addStepper(0, stepper1); // ось 0 28 | planner.addStepper(1, stepper2); // ось 1 29 | 30 | // устанавливаем ускорение и скорость 31 | planner.setAcceleration(100); 32 | planner.setMaxSpeed(300); 33 | 34 | planner.reset(); // сбрасываем все позиции в 0 (они и так в 0 при запуске) 35 | 36 | // массив с целевыми позициями осей, размер массива равен количеству осей 37 | int target[] = {300, 200}; 38 | 39 | // отправляем 40 | planner.setTarget(target); 41 | } 42 | 43 | void loop() { 44 | // здесь происходит движение моторов, вызывать как можно чаще 45 | planner.tick(); 46 | 47 | // вернёт true, если все моторы доехали 48 | if (planner.ready()) { 49 | // загружаем новую точку 50 | int newTarget[] = {10, 50}; 51 | planner.setTarget(newTarget); 52 | } 53 | 54 | // асинхронно вывожу в порт графики 55 | static uint32_t tmr; 56 | if (millis() - tmr >= 20) { 57 | tmr = millis(); 58 | Serial.print(stepper1.pos); 59 | Serial.print(','); 60 | Serial.println(stepper2.pos); 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /examples/Planner/PlannerHoming/PlannerHoming.ino: -------------------------------------------------------------------------------- 1 | // пример калибровки нуля по концевикам 2 | // концевики на D6 и D7 3 | #define LIMSW_X 6 4 | #define LIMSW_Y 7 5 | 6 | #include "GyverPlanner.h" 7 | Stepper stepper1(2, 5); 8 | Stepper stepper2(3, 6); 9 | GPlanner planner; 10 | 11 | void setup() { 12 | // добавляем шаговики на оси 13 | planner.addStepper(0, stepper1); // ось 0 14 | planner.addStepper(1, stepper2); // ось 1 15 | 16 | // устанавливаем ускорение и скорость 17 | planner.setAcceleration(100); 18 | planner.setMaxSpeed(300); 19 | 20 | // пуллапим. Кнопки замыкают на GND 21 | pinMode(LIMSW_X, INPUT_PULLUP); 22 | pinMode(LIMSW_Y, INPUT_PULLUP); 23 | } 24 | 25 | void loop() { 26 | } 27 | 28 | void homing() { 29 | if (digitalRead(LIMSW_X)) { // если концевик X не нажат 30 | planner.setSpeed(0, -10); // ось Х, -10 шаг/сек 31 | while (digitalRead(LIMSW_X)) { // пока кнопка не нажата 32 | planner.tick(); // крутим 33 | } 34 | // кнопка нажалась - покидаем цикл 35 | planner.brake(); // тормозим, приехали 36 | } 37 | 38 | if (digitalRead(LIMSW_Y)) { // если концевик Y не нажат 39 | planner.setSpeed(1, -10); // ось Y, -10 шаг/сек 40 | while (digitalRead(LIMSW_Y)) { // пока кнопка не нажата 41 | planner.tick(); // крутим 42 | } 43 | // кнопка нажалась - покидаем цикл 44 | planner.brake(); // тормозим, приехали 45 | } 46 | planner.reset(); // сбрасываем координаты в 0 47 | } 48 | -------------------------------------------------------------------------------- /examples/Planner/PlannerSpeedControl/PlannerSpeedControl.ino: -------------------------------------------------------------------------------- 1 | // крутим мотор. Отправляй в сериал целое число, шаг/сек 2 | 3 | #include "GyverPlanner.h" 4 | Stepper stepper1(2, 5); 5 | Stepper stepper2(3, 6); 6 | GPlanner planner; 7 | 8 | void setup() { 9 | Serial.begin(57600); 10 | // добавляем шаговики на оси 11 | planner.addStepper(0, stepper1); // ось 0 12 | planner.addStepper(1, stepper2); // ось 1 13 | 14 | Serial.setTimeout(10); 15 | } 16 | 17 | void loop() { 18 | // здесь происходит движение моторов, вызывать как можно чаще 19 | planner.tick(); 20 | 21 | // управляем скоростью 22 | if (Serial.available() > 0) { 23 | int val = Serial.parseInt(); 24 | planner.setSpeed(0, val); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /examples/Planner/PlannerTimerISR/PlannerTimerISR.ino: -------------------------------------------------------------------------------- 1 | // пример с движением моторов в прерывании 2 | // используется Timer1 на atmega328 3 | 4 | #include "GyverPlanner.h" 5 | Stepper stepper1(2, 5); 6 | Stepper stepper2(3, 6); 7 | GPlanner planner; 8 | 9 | void setup() { 10 | Serial.begin(115200); 11 | // добавляем шаговики на оси 12 | planner.addStepper(0, stepper1); // ось 0 13 | planner.addStepper(1, stepper2); // ось 1 14 | 15 | initTimer(); 16 | startTimer(); 17 | } 18 | 19 | // прерывание таймера 20 | ISR(TIMER1_COMPA_vect) { 21 | // здесь происходит движение моторов 22 | // если мотор должен двигаться (true) - ставим новый период таймеру 23 | if (planner.tickManual()) setPeriod(planner.getPeriod()); 24 | else stopTimer(); 25 | // если нет - останавливаем таймер 26 | } 27 | 28 | int path[][2] = { 29 | {10, 10}, 30 | {100, 5}, 31 | {200, 200}, 32 | {150, 150}, 33 | {0, 0}, 34 | {0, 50}, 35 | }; 36 | 37 | int count = 0; // счётчик точек маршрута 38 | 39 | void loop() { 40 | // вернёт true, если все моторы доехали 41 | if (planner.ready()) { 42 | if (count < sizeof(path) / 4) { // ограничиваем на количество точек 43 | if (planner.setTarget(path[count])) { // загружаем новую точку (начнётся с 0) 44 | // выполняем дальше код, если есть куда двигаться (получили true) 45 | // после вызова setTarget обновляется период! можно его юзать 46 | startTimer(); // запускаем таймер 47 | setPeriod(planner.getPeriod()); // устанавливаем период 48 | count++; 49 | } 50 | } 51 | } 52 | 53 | // асинхронно вывожу в порт графики 54 | static uint32_t tmr; 55 | if (millis() - tmr >= 20) { 56 | tmr = millis(); 57 | Serial.print(stepper1.pos); 58 | Serial.print(','); 59 | Serial.println(stepper2.pos); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /examples/Planner/PlannerTimerISR/timer.ino: -------------------------------------------------------------------------------- 1 | // настроить таймер 2 | void initTimer() { 3 | TCCR1A = 0; 4 | // CTC по OCR1A, делитель /64 5 | TCCR1B = bit(WGM12) | 0b011; 6 | } 7 | 8 | // установить период 9 | void setPeriod(uint32_t prd) { 10 | // один тик таймера - 4 мкс (при 16 МГц клоке) 11 | OCR1A = (uint32_t)prd >> 2; 12 | } 13 | 14 | // запустить и сбросить таймер 15 | void startTimer() { 16 | TIMSK1 = bit(OCIE1A); 17 | TCNT1 = 0; 18 | } 19 | 20 | // остановить таймер 21 | void stopTimer() { 22 | TIMSK1 = 0; 23 | TCNT1 = 0; 24 | } 25 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerArray/PlannerArray.ino: -------------------------------------------------------------------------------- 1 | // пример с записанным в памяти маршрутом 2 | // смотри график 3 | 4 | int path[][2] = { 5 | {100, 250}, 6 | {160, 30}, 7 | {230, 250}, 8 | {60, 100}, 9 | {270, 100}, 10 | }; 11 | 12 | // количество точек (пусть компилятор сам считает) 13 | // как вес всего массива / (2+2) байта 14 | int nodeAmount = sizeof(path) / 4; 15 | 16 | #include "GyverPlanner2.h" 17 | Stepper stepper1(2, 5); 18 | Stepper stepper2(3, 6); 19 | GPlanner2 planner; 20 | 21 | void setup() { 22 | Serial.begin(115200); 23 | // добавляем шаговики на оси 24 | planner.addStepper(0, stepper1); // ось 0 25 | planner.addStepper(1, stepper2); // ось 1 26 | 27 | // устанавливаем ускорение и скорость 28 | planner.setAcceleration(500); 29 | planner.setMaxSpeed(500); 30 | 31 | // начальная точка системы должна совпадать с первой точкой маршрута 32 | planner.setCurrent(path[0]); 33 | planner.start(); 34 | } 35 | 36 | int count = 0; // счётчик точек маршрута 37 | void loop() { 38 | // здесь происходит движение моторов, вызывать как можно чаще 39 | planner.tick(); 40 | 41 | // если в буфере планировщика есть место 42 | if (planner.available()) { 43 | // добавляем точку маршрута и является ли она точкой остановки (0 - нет) 44 | planner.addTarget(path[count], 0); 45 | if (++count >= sizeof(path) / 4) count = 0; // закольцевать 46 | } 47 | 48 | // асинхронно вывожу в порт графики 49 | static uint32_t tmr; 50 | if (millis() - tmr >= 20) { 51 | tmr = millis(); 52 | Serial.print(stepper1.pos); 53 | Serial.print(','); 54 | Serial.println(stepper2.pos); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerArray/stepperPlot/stepperPlot.pde: -------------------------------------------------------------------------------- 1 | String port = "COM6"; // имя порта 2 | 3 | import processing.serial.*; 4 | Serial myPort; 5 | 6 | void setup() { 7 | size(500, 500); 8 | myPort = new Serial(this, port, 115200); 9 | fill(0); 10 | noStroke(); 11 | } 12 | 13 | void draw() { 14 | if (myPort.available() > 0) { 15 | String str = myPort.readStringUntil('\n').trim(); 16 | if (str != null) { 17 | String[] pos = split(str, ','); 18 | if (pos.length < 2) return; 19 | int x = int(pos[0]); 20 | int y = int(pos[1]); 21 | fill(10); 22 | circle(x+10, y+10, 4); 23 | fill(200, 2); 24 | rect(0, 0, width, height); 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerCircle/PlannerCircle.ino: -------------------------------------------------------------------------------- 1 | // пример с записанным в памяти маршрутом окружности 2 | // работу можно посмотреть в плоттере, а лучше в приложенном stepperPlot для Processing 3 | 4 | const int pointAm = 30; // количество точек в круге 5 | int radius = 100; // радиус круга 6 | int32_t path[pointAm + 1 + 1][2]; // буфер круга 7 | // +1 на стартовую точку +1 на замыкание круга 8 | 9 | #include "GyverPlanner2.h" 10 | Stepper stepper1(2, 5); 11 | Stepper stepper2(3, 6); 12 | GPlanner2 planner; 13 | 14 | void setup() { 15 | Serial.begin(115200); 16 | // добавляем шаговики на оси 17 | planner.addStepper(0, stepper1); // ось 0 18 | planner.addStepper(1, stepper2); // ось 1 19 | 20 | // устанавливаем ускорение и скорость 21 | planner.setAcceleration(500); 22 | planner.setMaxSpeed(500); 23 | 24 | // начальная точка системы должна совпадать с первой точкой маршрута 25 | planner.setCurrent(path[0]); 26 | 27 | // заполняем буфер 28 | for (int i = 0; i <= pointAm; i++) { 29 | path[i + 1][0] = radius + radius * cos(TWO_PI * i / pointAm); 30 | path[i + 1][1] = radius + radius * sin(TWO_PI * i / pointAm); 31 | } 32 | // 0 - координата 0,0 33 | // 1 - первая координата круга 34 | // итд 35 | 36 | planner.start(); 37 | } 38 | 39 | int count = 0; // счётчик точек маршрута 40 | void loop() { 41 | // здесь происходит движение моторов, вызывать как можно чаще 42 | planner.tick(); 43 | 44 | // если в буфере планировщика есть место 45 | if (planner.available()) { 46 | // добавляем точку маршрута и является ли она точкой остановки (0 - нет) 47 | planner.addTarget(path[count], 0); 48 | if (++count >= sizeof(path) / 8) count = 0; // закольцевать 49 | } 50 | 51 | // асинхронно вывожу в порт графики 52 | static uint32_t tmr; 53 | if (millis() - tmr >= 20) { 54 | tmr = millis(); 55 | Serial.print(stepper1.pos); 56 | Serial.print(','); 57 | Serial.println(stepper2.pos); 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerCircle/stepperPlot/stepperPlot.pde: -------------------------------------------------------------------------------- 1 | String port = "COM6"; // имя порта 2 | 3 | import processing.serial.*; 4 | Serial myPort; 5 | 6 | void setup() { 7 | size(500, 500); 8 | myPort = new Serial(this, port, 115200); 9 | fill(0); 10 | noStroke(); 11 | } 12 | 13 | void draw() { 14 | if (myPort.available() > 0) { 15 | String str = myPort.readStringUntil('\n').trim(); 16 | if (str != null) { 17 | String[] pos = split(str, ','); 18 | if (pos.length < 2) return; 19 | int x = int(pos[0]); 20 | int y = int(pos[1]); 21 | fill(10); 22 | circle(x+10, y+10, 4); 23 | fill(200, 2); 24 | rect(0, 0, width, height); 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerCircleISR/PlannerCircleISR.ino: -------------------------------------------------------------------------------- 1 | // тож самое, но тик по таймеру 2 | // пример с записанным в памяти маршрутом окружности 3 | // работу можно посмотреть в плоттере, а лучше в приложенном stepperPlot для Processing 4 | 5 | const int pointAm = 30; // количество точек в круге 6 | int radius = 100; // радиус круга 7 | int32_t path[pointAm + 1 + 1][2]; // буфер круга 8 | // +1 на стартовую точку +1 на замыкание круга 9 | 10 | #include "GyverPlanner2.h" 11 | Stepper stepper1(2, 5); 12 | Stepper stepper2(3, 6); 13 | GPlanner2 planner; 14 | 15 | void setup() { 16 | Serial.begin(115200); 17 | // добавляем шаговики на оси 18 | planner.addStepper(0, stepper1); // ось 0 19 | planner.addStepper(1, stepper2); // ось 1 20 | 21 | // устанавливаем ускорение и скорость 22 | planner.setAcceleration(500); 23 | planner.setMaxSpeed(500); 24 | 25 | // начальная точка системы должна совпадать с первой точкой маршрута 26 | planner.setCurrent(path[0]); 27 | 28 | // заполняем буфер 29 | for (int i = 0; i <= pointAm; i++) { 30 | path[i + 1][0] = radius + radius * cos(TWO_PI * i / pointAm); 31 | path[i + 1][1] = radius + radius * sin(TWO_PI * i / pointAm); 32 | } 33 | // 0 - координата 0,0 34 | // 1 - первая координата круга 35 | // итд 36 | 37 | // заводим всё 38 | planner.start(); 39 | initTimer(); 40 | } 41 | 42 | // прерывание таймера 43 | ISR(TIMER1_COMPA_vect) { 44 | // здесь происходит движение моторов 45 | // если мотор должен двигаться (true) - ставим новый период таймеру 46 | if (planner.tickManual()) setPeriod(planner.getPeriod()); 47 | else stopTimer(); 48 | // если нет - останавливаем таймер 49 | } 50 | 51 | int count = 0; // счётчик точек маршрута 52 | void loop() { 53 | // вручную проверяем буфер. Если начался новый отрезок движения 54 | if (planner.checkBuffer()) { 55 | startTimer(); // запускаем таймер 56 | setPeriod(planner.getPeriod()); // устанавливаем новый период 57 | } 58 | 59 | // если в буфере планировщика есть место 60 | if (planner.available()) { 61 | // добавляем точку маршрута и является ли она точкой остановки (0 - нет) 62 | planner.addTarget(path[count], 0); 63 | if (++count >= sizeof(path) / 8) count = 0; // закольцевать 64 | } 65 | 66 | // асинхронно вывожу в порт графики 67 | static uint32_t tmr; 68 | if (millis() - tmr >= 20) { 69 | tmr = millis(); 70 | Serial.print(stepper1.pos); 71 | Serial.print(','); 72 | Serial.println(stepper2.pos); 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerCircleISR/stepperPlot/stepperPlot.pde: -------------------------------------------------------------------------------- 1 | String port = "COM6"; // имя порта 2 | 3 | import processing.serial.*; 4 | Serial myPort; 5 | 6 | void setup() { 7 | size(500, 500); 8 | myPort = new Serial(this, port, 115200); 9 | fill(0); 10 | noStroke(); 11 | } 12 | 13 | void draw() { 14 | if (myPort.available() > 0) { 15 | String str = myPort.readStringUntil('\n').trim(); 16 | if (str != null) { 17 | String[] pos = split(str, ','); 18 | if (pos.length < 2) return; 19 | int x = int(pos[0]); 20 | int y = int(pos[1]); 21 | fill(10); 22 | circle(x+10, y+10, 4); 23 | fill(200, 2); 24 | rect(0, 0, width, height); 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /examples/Planner2/PlannerCircleISR/timer.ino: -------------------------------------------------------------------------------- 1 | // настроить таймер 2 | void initTimer() { 3 | TCCR1A = 0; 4 | // CTC по OCR1A, делитель /64 5 | TCCR1B = bit(WGM12) | 0b011; 6 | } 7 | 8 | // установить период 9 | void setPeriod(uint32_t prd) { 10 | // один тик таймера - 4 мкс (при 16 МГц клоке) 11 | OCR1A = (uint32_t)prd >> 2; 12 | } 13 | 14 | // запустить и сбросить таймер 15 | void startTimer() { 16 | TIMSK1 = bit(OCIE1A); 17 | TCNT1 = 0; 18 | } 19 | 20 | // остановить таймер 21 | void stopTimer() { 22 | TIMSK1 = 0; 23 | TCNT1 = 0; 24 | } 25 | -------------------------------------------------------------------------------- /examples/Stepper/accelDeccelButton/accelDeccelButton.ino: -------------------------------------------------------------------------------- 1 | // начинаем плавный разгон при удержании кнопки. При отпускании тормозим 2 | 3 | //#define SMOOTH_ALGORITHM 4 | #include 5 | //GStepper stepper(2048, 5, 3, 4, 2); 6 | GStepper stepper(2048, 2, 5); 7 | 8 | 9 | void setup() { 10 | Serial.begin(115200); 11 | pinMode(A0, INPUT_PULLUP); // кнопка на A0 и GND 12 | stepper.enable(); 13 | stepper.autoPower(true); 14 | 15 | // установка ускорения в шагах/сек/сек 16 | stepper.setAcceleration(1200); 17 | stepper.setRunMode(KEEP_SPEED); 18 | } 19 | 20 | bool btnState = false; 21 | void loop() { 22 | stepper.tick(); 23 | 24 | // кнопка нажата 25 | if (!digitalRead(A0) && !btnState) { 26 | btnState = true; 27 | stepper.setSpeed(3000, SMOOTH); 28 | } 29 | 30 | // кнопка отпущена 31 | if (digitalRead(A0) && btnState) { 32 | btnState = false; 33 | stepper.stop(); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /examples/Stepper/demo/demo.ino: -------------------------------------------------------------------------------- 1 | // демо - основные возможности библиотеки 2 | 3 | #include 4 | GStepper stepper(2048, 5, 3, 4, 2); 5 | 6 | // мотор с драйвером ULN2003 подключается по порядку пинов, но крайние нужно поменять местами 7 | // то есть у меня подключено D2-IN1, D3-IN2, D4-IN3, D5-IN4, но в программе поменял 5 и 2 8 | 9 | // создание объекта 10 | // steps - шагов на один оборот вала (для расчётов с градусами) 11 | // step, dir, pin1, pin2, pin3, pin4 - любые GPIO 12 | // en - пин отключения драйвера, любой GPIO 13 | //GStepper stepper(steps, step, dir); // драйвер step-dir 14 | //GStepper stepper(steps, step, dir, en); // драйвер step-dir + пин enable 15 | //GStepper stepper(steps, pin1, pin2, pin3, pin4); // драйвер 4 пин 16 | //GStepper stepper(steps, pin1, pin2, pin3, pin4, en); // драйвер 4 пин + enable 17 | //GStepper stepper(steps, pin1, pin2, pin3, pin4); // драйвер 4 пин полушаг 18 | //GStepper stepper(steps, pin1, pin2, pin3, pin4, en); // драйвер 4 пин полушаг + enable 19 | 20 | void setup() { 21 | Serial.begin(115200); 22 | // режим поддержания скорости 23 | stepper.setRunMode(KEEP_SPEED); 24 | 25 | // можно установить скорость 26 | stepper.setSpeed(120); // в шагах/сек 27 | stepper.setSpeedDeg(80); // в градусах/сек 28 | 29 | // режим следования к целевй позиции 30 | stepper.setRunMode(FOLLOW_POS); 31 | 32 | // можно установить позицию 33 | stepper.setTarget(-2024); // в шагах 34 | stepper.setTargetDeg(-360); // в градусах 35 | 36 | // установка макс. скорости в градусах/сек 37 | stepper.setMaxSpeedDeg(400); 38 | 39 | // установка макс. скорости в шагах/сек 40 | stepper.setMaxSpeed(400); 41 | 42 | // установка ускорения в градусах/сек/сек 43 | stepper.setAccelerationDeg(300); 44 | 45 | // установка ускорения в шагах/сек/сек 46 | stepper.setAcceleration(300); 47 | 48 | // отключать мотор при достижении цели 49 | stepper.autoPower(true); 50 | 51 | // включить мотор (если указан пин en) 52 | stepper.enable(); 53 | } 54 | 55 | void loop() { 56 | // просто крутим туды-сюды 57 | if (!stepper.tick()) { 58 | static bool dir; 59 | dir = !dir; 60 | stepper.setTarget(dir ? -1024 : 1024); 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /examples/Stepper/endSwitch/endSwitch.ino: -------------------------------------------------------------------------------- 1 | // пример с концевиком 2 | #include 3 | //GStepper stepper(2048, 5, 3, 4, 2); 4 | GStepper stepper(2048, 2, 5); 5 | 6 | void setup() { 7 | // наша задача - при запуске крутить мотор в сторону до нажатия на кнопку 8 | pinMode(12, INPUT_PULLUP); // кнопка на D12 и GND 9 | 10 | stepper.setRunMode(KEEP_SPEED); 11 | stepper.setSpeedDeg(-10); // медленно крутимся НАЗАД 12 | 13 | // пока кнопка не нажата 14 | while(digitalRead(12)) { 15 | stepper.tick(); 16 | // yield(); // для esp8266 17 | } 18 | // вот тут кнопка нажата, сразу вырубаем мотор. 19 | // Текущее положение также сбрасывается в 0 20 | stepper.reset(); 21 | 22 | // дальше например врубаем FOLLOW_POS 23 | stepper.setRunMode(FOLLOW_POS); 24 | } 25 | 26 | void loop() { 27 | // и качаемся в 20 шагах от кнопки и до 300 28 | static bool dir; 29 | if (!stepper.tick()) { 30 | dir = !dir; 31 | stepper.setTarget(dir ? 20 : 300); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /examples/Stepper/externalDriver/externalDriver.ino: -------------------------------------------------------------------------------- 1 | // пример с использованием "внешнего" драйвера, который может быть 2 | // подключен к расширителю портов. 3 | 4 | // в качестве примера использую digitalWrite и родные пины 5 | #define PIN_A 2 6 | #define PIN_B 4 7 | #define PIN_C 3 8 | #define PIN_D 5 9 | 10 | #include 11 | GStepper stepper(2048); 12 | 13 | void setup() { 14 | Serial.begin(9600); 15 | // выходы 16 | pinMode(PIN_A, 1); 17 | pinMode(PIN_B, 1); 18 | pinMode(PIN_C, 1); 19 | pinMode(PIN_D, 1); 20 | 21 | stepper.setRunMode(KEEP_SPEED); // режим поддержания скорости 22 | stepper.setSpeedDeg(100); // в градусах/сек 23 | stepper.setAcceleration(500); 24 | 25 | // подключить свою функцию-обработчик шага 26 | stepper.attachStep(step); 27 | 28 | // подключить свою функцию-обработчик для управления питанием 29 | stepper.attachPower(pwr); 30 | 31 | stepper.autoPower(1); // включаем авто выкл питания 32 | } 33 | 34 | // наша функция-обработчик. Будет вызываться на каждом шагу 35 | // у STEPPER4WIRE val содержит состояния обмоток как 0bABCD 36 | // у STEPPER2WIRE val содержит 0 или 1 как DIR, STEP нужно дёрнуть самому 37 | void step(byte val) { 38 | // дёргаем вручную пины 39 | digitalWrite(PIN_D, val & 1); 40 | val >>= 1; 41 | digitalWrite(PIN_C, val & 1); 42 | val >>= 1; 43 | digitalWrite(PIN_B, val & 1); 44 | val >>= 1; 45 | digitalWrite(PIN_A, val & 1); 46 | } 47 | 48 | void pwr(bool val) { 49 | // тут val будет 0 или 1 в зависимости от питания. Подавай на EN 50 | Serial.println(val); 51 | } 52 | 53 | void loop() { 54 | stepper.tick(); 55 | 56 | // разгон и остановка каждые 3 секунды 57 | static uint32_t tmr; 58 | if (millis() - tmr >= 3000) { 59 | tmr = millis(); 60 | static bool dir = 1; 61 | dir = !dir; 62 | if (!dir) stepper.stop(); 63 | else stepper.setSpeedDeg(100); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /examples/Stepper/multiStepper/multiStepper.ino: -------------------------------------------------------------------------------- 1 | // крутим мотор туда-сюда плавно с ускорением 2 | 3 | #include "GyverStepper.h" 4 | // подключим три мотора 5 | // у первого и второго управление EN не подключаем 6 | GStepper stepper1(100, 2, 3); 7 | GStepper stepper2(100, 4, 5); 8 | GStepper stepper3(100, 6, 7, 8); 9 | 10 | void setup() { 11 | // мотор 1 просто вращается 12 | stepper1.setRunMode(KEEP_SPEED); 13 | stepper1.setSpeed(300); 14 | 15 | // мотор 2 будет делать sweep по проверке tick 16 | stepper2.setRunMode(FOLLOW_POS); 17 | stepper2.setMaxSpeed(1000); 18 | stepper2.setAcceleration(300); 19 | 20 | // мотор 3 будет перемещаться на случайную позицию 21 | stepper3.setRunMode(FOLLOW_POS); 22 | stepper3.setMaxSpeed(1000); 23 | stepper3.setAcceleration(300); 24 | stepper3.autoPower(true); 25 | stepper3.enable(); 26 | } 27 | 28 | void loop() { 29 | // первый мотор 30 | stepper1.tick(); 31 | 32 | // второй крутим туды-сюды (-1000, 1000) 33 | if (!stepper2.tick()) { 34 | static bool dir; 35 | dir = !dir; 36 | stepper2.setTarget(dir ? -1000 : 1000); 37 | } 38 | 39 | // третий по таймеру 40 | // будет отключаться при остановке 41 | stepper3.tick(); 42 | static uint32_t tmr; 43 | if (millis() - tmr > 5000) { // каждые 5 секунд 44 | tmr = millis(); 45 | stepper3.setTarget(random(0, 2000)); // рандом 0-2000 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /examples/Stepper/potPos/potPos.ino: -------------------------------------------------------------------------------- 1 | // установка позиции потенциометром 2 | 3 | #include 4 | //GStepper stepper(2048, 5, 3, 4, 2); 5 | GStepper stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(115200); 9 | stepper.setRunMode(FOLLOW_POS); // режим следования к целевй позиции 10 | stepper.setMaxSpeed(400); // установка макс. скорости в шагах/сек 11 | stepper.setAcceleration(500); // установка ускорения в шагах/сек/сек 12 | 13 | // пусть драйвер выключается при достижении позиции 14 | stepper.autoPower(true); 15 | } 16 | 17 | void loop() { 18 | stepper.tick(); 19 | 20 | // сделаем таймер на 20 мс 21 | // будем опрашивать потенциометр и строить графики 22 | static uint32_t tmr2; 23 | if (millis() - tmr2 > 20) { 24 | tmr2 = millis(); 25 | static float val; 26 | // потенциометр на A0 27 | // фильтруем, иначе мотор будет трястись 28 | val += (analogRead(0) - val) * 0.08; 29 | 30 | stepper.setTarget(val); // ставим новую позицию 31 | Serial.print(stepper.getTarget()); 32 | Serial.print(','); 33 | Serial.println(stepper.getCurrent()); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /examples/Stepper/potSpeed/potSpeed.ino: -------------------------------------------------------------------------------- 1 | // установка скорости потенциометром 2 | 3 | #include 4 | //GStepper stepper(2048, 5, 3, 4, 2); 5 | GStepper stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | stepper.setRunMode(KEEP_SPEED); // режим поддержания скорости 9 | stepper.setSpeedDeg(50); // в градусах/сек 10 | } 11 | 12 | void loop() { 13 | stepper.tick(); 14 | 15 | // сделаем таймер на 50 мс и будем опрашивать потенциометр 16 | // менять скорость чаще нет смысла 17 | static uint32_t tmr2; 18 | if (millis() - tmr2 > 50) { 19 | tmr2 = millis(); 20 | 21 | // ставим новую скорость (-512.. 512 шагов в секунду) 22 | // будет крутиться в разные стороны 23 | stepper.setSpeed(512 - analogRead(0)); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /examples/Stepper/smoothAlgorithm/smoothAlgorithm.ino: -------------------------------------------------------------------------------- 1 | // используем более плавный алгоритм движения. Макс. скорость ограничена до 2 | // 7000 шаг/сек, алгоритм использует много процессорного времени! 3 | 4 | // перед подключением библиотеки дефайним 5 | #define SMOOTH_ALGORITHM 6 | 7 | #include 8 | //GStepper stepper(2048, 5, 3, 4, 2); 9 | GStepper stepper(2048, 2, 5); 10 | 11 | void setup() { 12 | Serial.begin(115200); 13 | 14 | // режим следования к целевй позиции 15 | stepper.setRunMode(FOLLOW_POS); 16 | 17 | // установка макс. скорости в шагах/сек 18 | stepper.setMaxSpeed(400); 19 | 20 | // установка ускорения в шагах/сек/сек 21 | stepper.setAcceleration(500); 22 | } 23 | 24 | void loop() { 25 | // просто крутим туды-сюды 26 | if (!stepper.tick()) { 27 | static bool dir; 28 | dir = !dir; 29 | stepper.setTarget(dir ? -400 : 400); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /examples/Stepper/speed/speed.ino: -------------------------------------------------------------------------------- 1 | // крутимся с заданной скоростью 2 | 3 | #include 4 | //GStepper stepper(2048, 5, 3, 4, 2); 5 | GStepper stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | stepper.setRunMode(KEEP_SPEED); // режим поддержания скорости 9 | stepper.setSpeedDeg(50); // в градусах/сек 10 | } 11 | 12 | void loop() { 13 | stepper.tick(); 14 | } 15 | -------------------------------------------------------------------------------- /examples/Stepper/speedSerialControl/speedSerialControl.ino: -------------------------------------------------------------------------------- 1 | // управляем скоростью из СОМ порта 2 | // отправь q для тормоза 3 | // отправь w для плавной остановки 4 | // отправь e для скорости 5 град/сек 5 | // отправь r для скорости 100 град/сек 6 | 7 | #include 8 | //GStepper stepper(2048, 5, 3, 4, 2); 9 | GStepper stepper(2048, 2, 5); 10 | 11 | void setup() { 12 | Serial.begin(9600); 13 | stepper.setRunMode(KEEP_SPEED); // режим поддержания скорости 14 | stepper.setSpeedDeg(5); // в градусах/сек 15 | } 16 | 17 | void loop() { 18 | stepper.tick(); 19 | if (Serial.available()) { 20 | char ch = Serial.read(); 21 | if (ch == 'q') stepper.brake(); 22 | if (ch == 'w') stepper.stop(); 23 | if (ch == 'e') stepper.setSpeedDeg(5); 24 | if (ch == 'r') stepper.setSpeedDeg(100); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /examples/Stepper/stop/stop.ino: -------------------------------------------------------------------------------- 1 | // пример работы stop() 2 | // на графике будет видно, как сместилась установка и мотор к ней затормозил 3 | #include 4 | //GStepper stepper(2048, 5, 3, 4, 2); 5 | GStepper stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(115200); 9 | stepper.setMaxSpeed(400); 10 | stepper.setAcceleration(300); 11 | stepper.setRunMode(FOLLOW_POS); 12 | 13 | // отправляем мотор подальше 14 | stepper.setTarget(-2024); 15 | } 16 | 17 | void loop() { 18 | stepper.tick(); 19 | 20 | // плавная остановка через 3 секунды 21 | static uint32_t tmr; 22 | static bool flag = false; 23 | if (!flag && millis() > 3000) { 24 | stepper.stop(); 25 | flag = true; 26 | } 27 | 28 | // график установки и текущей позиции 29 | static uint32_t tmr2; 30 | if (millis() - tmr2 > 20) { 31 | tmr2 = millis(); 32 | Serial.print(stepper.getTarget()); 33 | Serial.print(','); 34 | Serial.println(stepper.getCurrent()); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /examples/Stepper/sweep/sweep.ino: -------------------------------------------------------------------------------- 1 | // крутим мотор туда-сюда плавно с ускорением 2 | 3 | #include 4 | //GStepper stepper(2048, 5, 3, 4, 2); 5 | GStepper stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(115200); 9 | 10 | // режим следования к целевй позиции 11 | stepper.setRunMode(FOLLOW_POS); 12 | 13 | // установка макс. скорости в шагах/сек 14 | stepper.setMaxSpeed(400); 15 | 16 | // установка ускорения в шагах/сек/сек 17 | stepper.setAcceleration(500); 18 | } 19 | 20 | void loop() { 21 | // просто крутим туды-сюды 22 | if (!stepper.tick()) { 23 | static bool dir; 24 | dir = !dir; 25 | stepper.setTarget(dir ? -400 : 400); 26 | } 27 | 28 | // график положения 29 | static uint32_t tmr2; 30 | if (millis() - tmr2 > 20) { 31 | tmr2 = millis(); 32 | Serial.println(stepper.getCurrent()); 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /examples/Stepper/timerISR/timerISR.ino: -------------------------------------------------------------------------------- 1 | // пример с тиком по прерыванию таймера 2 | // используется GyverTimers 3 | 4 | #include 5 | //GStepper stepper(2048, 5, 3, 4, 2); 6 | GStepper stepper(2048, 2, 5); 7 | 8 | #include 9 | 10 | void setup() { 11 | Serial.begin(115200); 12 | 13 | // режим следования к целевй позиции 14 | stepper.setRunMode(FOLLOW_POS); 15 | 16 | // установка макс. скорости в шагах/сек 17 | stepper.setMaxSpeed(400); 18 | 19 | // установка ускорения в шагах/сек/сек 20 | stepper.setAcceleration(500); 21 | 22 | // настраиваем прерывания с периодом, при котором 23 | // система сможет обеспечить максимальную скорость мотора. 24 | // Для большей плавности лучше лучше взять период чуть меньше, например в два раза 25 | Timer2.setPeriod(stepper.getMinPeriod() / 2); 26 | 27 | // взводим прерывание 28 | Timer2.enableISR(); 29 | } 30 | 31 | // обработчик 32 | ISR(TIMER2_A) { 33 | stepper.tick(); // тикаем тут 34 | } 35 | 36 | void loop() { 37 | // просто крутим туды-сюды 38 | if (!stepper.tick()) { // тут всё равно вызываем для смены направления 39 | static bool dir; 40 | dir = !dir; 41 | stepper.setTarget(dir ? -400 : 400); 42 | } 43 | 44 | // график положения 45 | Serial.println(stepper.getCurrent()); 46 | 47 | // задержка, чтобы показать работу степпера в прерывании 48 | delay(100); 49 | } 50 | -------------------------------------------------------------------------------- /examples/Stepper2/SpeedControl/SpeedControl.ino: -------------------------------------------------------------------------------- 1 | // крутим мотор. Отправляй в сериал целое число, шаг/сек 2 | 3 | #include "GyverStepper2.h" 4 | //GStepper2 stepper(2048, 5, 3, 4, 2); 5 | GStepper2 stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(9600); 9 | Serial.setTimeout(10); 10 | } 11 | 12 | void loop() { 13 | // здесь происходит движение моторов, вызывать как можно чаще 14 | stepper.tick(); 15 | 16 | // управляем скоростью 17 | if (Serial.available() > 0) { 18 | int val = Serial.parseInt(); 19 | stepper.setSpeed(val); 20 | } 21 | 22 | static uint32_t tmr; 23 | if (millis() - tmr >= 30) { 24 | tmr = millis(); 25 | Serial.println(stepper.pos); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /examples/Stepper2/StepperControl/StepperControl.ino: -------------------------------------------------------------------------------- 1 | // здесь у нас моторы движутся по трём точкам траектории 2 | // можно открыть плоттер, наблюать за этим и отправлять команды: 3 | // s - стоп 4 | // b - тормоз 5 | // p - пауза 6 | // r - продолжить 7 | //#define GS_NO_ACCEL 8 | #include "GyverStepper2.h" 9 | //GStepper2 stepper(2048, 5, 3, 4, 2); 10 | GStepper2 stepper(2048, 2, 5); 11 | 12 | void setup() { 13 | Serial.begin(9600); 14 | 15 | // устанавливаем ускорение и скорость 16 | stepper.setAcceleration(200); 17 | stepper.setMaxSpeed(100); 18 | stepper.setTarget(0); 19 | //stepper.setSpeed(100); 20 | } 21 | 22 | byte count = 0; 23 | int16_t path[] = {0, 200, 100}; 24 | 25 | void loop() { 26 | // здесь происходит движение мотора, вызывать как можно чаще 27 | stepper.tick(); 28 | 29 | // вернёт true, если все моторы доехали 30 | if (stepper.ready()) { 31 | stepper.setTarget(path[count]); // загружаем новую точку (начнётся с 0) 32 | if (++count >= sizeof(path) / 2) count = 0; 33 | } 34 | 35 | // управляем процессом 36 | if (Serial.available() > 0) { 37 | char incoming = Serial.read(); 38 | switch (incoming) { 39 | case 's': stepper.stop(); break; 40 | case 'b': stepper.brake(); break; 41 | case 'r': stepper.resume(); break; 42 | case 'p': stepper.pause(); break; 43 | } 44 | } 45 | 46 | // асинхронно вывожу в порт графики 47 | static uint32_t tmr; 48 | if (millis() - tmr >= 20) { 49 | tmr = millis(); 50 | Serial.print(stepper.getTarget()); 51 | Serial.print(','); 52 | Serial.println(stepper.pos); 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /examples/Stepper2/homing/homing.ino: -------------------------------------------------------------------------------- 1 | // пример калибровки нуля по концевикам 2 | // концевик на D6 3 | #define LIMSW_X 6 4 | 5 | #include "GyverStepper2.h" 6 | //GStepper2 stepper(2048, 5, 3, 4, 2); 7 | GStepper2 stepper(2048, 2, 5); 8 | 9 | void setup() { 10 | // пуллапим. Кнопки замыкают на GND 11 | pinMode(LIMSW_X, INPUT_PULLUP); 12 | } 13 | 14 | void loop() { 15 | } 16 | 17 | void homing() { 18 | if (digitalRead(LIMSW_X)) { // если концевик X не нажат 19 | stepper.setSpeed(-10); // ось Х, -10 шаг/сек 20 | while (digitalRead(LIMSW_X)) { // пока кнопка не нажата 21 | stepper.tick(); // крутим 22 | } 23 | // кнопка нажалась - покидаем цикл 24 | stepper.brake(); // тормозим, приехали 25 | } 26 | stepper.reset(); // сбрасываем координаты в 0 27 | } 28 | -------------------------------------------------------------------------------- /examples/Stepper2/sweep/sweep.ino: -------------------------------------------------------------------------------- 1 | // крутим туда сюда, тикаем в loop 2 | 3 | #include "GyverStepper2.h" 4 | //GStepper2 stepper(2048, 5, 3, 4, 2); 5 | GStepper2 stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(9600); 9 | //stepper.enable(); 10 | stepper.setMaxSpeed(100); // скорость движения к цели 11 | stepper.setAcceleration(200); // ускорение 12 | stepper.setTarget(300); // цель 13 | } 14 | 15 | bool dir = 1; 16 | void loop() { 17 | stepper.tick(); // мотор асинхронно крутится тут 18 | 19 | // если приехали 20 | if (stepper.ready()) { 21 | dir = !dir; // разворачиваем 22 | stepper.setTarget(dir * 300); // едем в другую сторону 23 | } 24 | 25 | // асинхронный вывод в порт 26 | static uint32_t tmr; 27 | if (millis() - tmr >= 30) { 28 | tmr = millis(); 29 | Serial.println(stepper.pos); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /examples/Stepper2/sweepISR/sweepISR.ino: -------------------------------------------------------------------------------- 1 | // крутим туда сюда, прерывание таймера 2 | 3 | #include "GyverStepper2.h" 4 | //GStepper2 stepper(2048, 5, 3, 4, 2); 5 | GStepper2 stepper(2048, 2, 5); 6 | 7 | void setup() { 8 | Serial.begin(9600); 9 | initTimer(); 10 | //stepper.enable(); 11 | stepper.setMaxSpeed(100); // скорость движения к цели 12 | stepper.setAcceleration(200); // ускорение 13 | stepper.setTarget(300); // цель 14 | setPeriod(stepper.getPeriod()); 15 | startTimer(); 16 | } 17 | 18 | // прерывание таймера 19 | ISR(TIMER1_COMPA_vect) { 20 | // здесь происходит движение мотора 21 | // если мотор должен двигаться (true) - ставим новый период таймеру 22 | if (stepper.tickManual()) setPeriod(stepper.getPeriod()); 23 | else stopTimer(); 24 | // если нет - останавливаем таймер 25 | } 26 | 27 | bool dir = 1; 28 | void loop() { 29 | // если приехали 30 | if (stepper.ready()) { 31 | dir = !dir; // разворачиваем 32 | stepper.setTarget(dir * 300); // едем в другую сторону 33 | setPeriod(stepper.getPeriod()); 34 | startTimer(); 35 | } 36 | 37 | // асинхронный вывод в порт 38 | static uint32_t tmr; 39 | if (millis() - tmr >= 30) { 40 | tmr = millis(); 41 | Serial.println(stepper.pos); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /examples/Stepper2/sweepISR/timer.ino: -------------------------------------------------------------------------------- 1 | // настроить таймер 2 | void initTimer() { 3 | TCCR1A = 0; 4 | // CTC по OCR1A, делитель /64 5 | TCCR1B = bit(WGM12) | 0b011; 6 | } 7 | 8 | // установить период 9 | void setPeriod(uint32_t prd) { 10 | // один тик таймера - 4 мкс (при 16 МГц клоке) 11 | OCR1A = (uint32_t)prd >> 2; 12 | } 13 | 14 | // запустить и сбросить таймер 15 | void startTimer() { 16 | TIMSK1 = bit(OCIE1A); 17 | TCNT1 = 0; 18 | } 19 | 20 | // остановить таймер 21 | void stopTimer() { 22 | TIMSK1 = 0; 23 | TCNT1 = 0; 24 | } 25 | -------------------------------------------------------------------------------- /examples/Stepper2/sweep_FAST_PROFILE/sweep_FAST_PROFILE.ino: -------------------------------------------------------------------------------- 1 | // крутим туда сюда, тикаем в loop 2 | 3 | // включаем быстрый профиль, 10 участков 4 | #define GS_FAST_PROFILE 10 5 | #include "GyverStepper2.h" 6 | //GStepper2 stepper(2048, 5, 3, 4, 2); 7 | GStepper2 stepper(2048, 2, 5); 8 | 9 | uint32_t tar = 60000; 10 | bool dir = 1; 11 | 12 | void setup() { 13 | Serial.begin(9600); 14 | 15 | //stepper.enable(); 16 | stepper.setMaxSpeed(30000); // скорость движения к цели 17 | stepper.setAcceleration(30000); // ускорение 18 | 19 | stepper.setTarget(tar); // цель 20 | } 21 | 22 | void loop() { 23 | while (1) { 24 | stepper.tick(); // мотор асинхронно крутится тут 25 | 26 | // если приехали 27 | if (stepper.ready()) { 28 | dir = !dir; // разворачиваем 29 | stepper.setTarget(dir * tar); // едем в другую сторону 30 | } 31 | 32 | // асинхронный вывод в порт 33 | static uint32_t tmr; 34 | if (millis() - tmr >= 30) { 35 | tmr = millis(); 36 | Serial.println(stepper.pos); 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For GyverStepper 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | GyverStepper KEYWORD1 9 | GyverStepper2 KEYWORD1 10 | GyverPlanner KEYWORD1 11 | GyverPlanner2 KEYWORD1 12 | StepperCore KEYWORD1 13 | 14 | GStepper KEYWORD1 15 | GStepper2 KEYWORD1 16 | GPlanner KEYWORD1 17 | GPlanner2 KEYWORD1 18 | Stepper KEYWORD1 19 | 20 | GS_NO_ACCEL KEYWORD1 21 | GS_FAST_PROFILE KEYWORD1 22 | 23 | ####################################### 24 | # Methods and Functions (KEYWORD2) 25 | ####################################### 26 | tick KEYWORD2 27 | reverse KEYWORD2 28 | invertEn KEYWORD2 29 | setCurrent KEYWORD2 30 | setCurrentDeg KEYWORD2 31 | getCurrent KEYWORD2 32 | getCurrentDeg KEYWORD2 33 | setTarget KEYWORD2 34 | setTargetDeg KEYWORD2 35 | getTarget KEYWORD2 36 | getTargetDeg KEYWORD2 37 | setMaxSpeed KEYWORD2 38 | setMaxSpeedDeg KEYWORD2 39 | setAcceleration KEYWORD2 40 | setAccelerationDeg KEYWORD2 41 | autoPower KEYWORD2 42 | stop KEYWORD2 43 | brake KEYWORD2 44 | reset KEYWORD2 45 | setSpeed KEYWORD2 46 | setSpeedDeg KEYWORD2 47 | getSpeed KEYWORD2 48 | getSpeedDeg KEYWORD2 49 | setRunMode KEYWORD2 50 | enable KEYWORD2 51 | disable KEYWORD2 52 | power KEYWORD2 53 | getState KEYWORD2 54 | home KEYWORD2 55 | getMinPeriod KEYWORD2 56 | stepTime KEYWORD2 57 | degPerMinute KEYWORD2 58 | degPerHour KEYWORD2 59 | attachStep KEYWORD2 60 | attachPower KEYWORD2 61 | setBacklash KEYWORD2 62 | step KEYWORD2 63 | 64 | addStepper KEYWORD2 65 | ready KEYWORD2 66 | pause KEYWORD2 67 | resume KEYWORD2 68 | tickManual KEYWORD2 69 | getPeriod KEYWORD2 70 | getStatus KEYWORD2 71 | 72 | clearBuffer KEYWORD2 73 | checkBuffer KEYWORD2 74 | addTarget KEYWORD2 75 | available KEYWORD2 76 | setDtA KEYWORD2 77 | start KEYWORD2 78 | 79 | ####################################### 80 | # Constants (LITERAL1) 81 | ####################################### 82 | SMOOTH LITERAL1 83 | NO_SMOOTH LITERAL1 84 | ABSOLUTE LITERAL1 85 | RELATIVE LITERAL1 86 | FOLLOW_POS LITERAL1 87 | KEEP_SPEED LITERAL1 88 | STEPPER2WIRE LITERAL1 89 | STEPPER4WIRE LITERAL1 90 | STEPPER4WIRE_HALF LITERAL1 91 | SMOOTH_ALGORITHM LITERAL1 92 | DRIVER_STEP_TIME LITERAL1 93 | STEPPER_PINS LITERAL1 94 | STEPPER_VIRTUAL LITERAL1 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=GyverStepper 2 | version=2.7 3 | author=AlexGyver 4 | maintainer=AlexGyver 5 | sentence=Fast library for stepmotor control and multi-axis planning 6 | paragraph=Fast library for stepmotor control and multi-axis planning 7 | category=Device Control 8 | url=https://github.com/GyverLibs/GyverStepper 9 | architectures=* -------------------------------------------------------------------------------- /src/FIFO.h: -------------------------------------------------------------------------------- 1 | // FIFO буфер для планировщика 2 2 | 3 | #ifndef _FIFO_h 4 | #define _FIFO_h 5 | #define FIFO_WIPE 1 6 | 7 | template 8 | class FIFO { 9 | public: 10 | FIFO() { 11 | for (uint16_t i = 0; i < SIZE; i++) buf[i] = 0; 12 | } 13 | // очистить 14 | void clear() { 15 | head = tail = 0; 16 | } 17 | 18 | // элементов в буфере 19 | uint16_t available() { 20 | return (SIZE + head - tail) % SIZE; 21 | } 22 | 23 | // прочитать элемент под номером от начала 24 | T get(int16_t i = 0) { 25 | return buf[(SIZE + tail + i) % SIZE]; 26 | } 27 | 28 | // прочитать крайний элемент буфера 29 | T getLast() { 30 | return buf[(SIZE + head - 1) % SIZE]; 31 | } 32 | 33 | // добавить элемент с конца 34 | void add(T data) { 35 | uint16_t i = (head + 1) % SIZE; 36 | if (i != tail) { 37 | buf[head] = data; 38 | head = i; 39 | } 40 | } 41 | 42 | // установить значение элемента под номером от начала 43 | void set(int16_t i, T data) { 44 | buf[(SIZE + tail + i) % SIZE] = data; 45 | } 46 | 47 | // сдвинуть начало на 1 48 | void next(bool wipe = 0) { 49 | if (wipe) buf[tail] = 0; 50 | tail = (tail + 1) % SIZE; 51 | } 52 | 53 | // доступность для записи (свободное место) 54 | bool availableForWrite() { 55 | return ((head + 1) % SIZE != tail); 56 | } 57 | 58 | private: 59 | T buf[SIZE]; 60 | uint16_t head = 0, tail = 0; 61 | }; 62 | 63 | #endif -------------------------------------------------------------------------------- /src/GStypes.h: -------------------------------------------------------------------------------- 1 | #ifndef _GStypes_h 2 | #define _GStypes_h 3 | enum GS_driverType { 4 | STEPPER2WIRE, 5 | STEPPER4WIRE, 6 | STEPPER4WIRE_HALF, 7 | STEPPER_PINS, 8 | STEPPER_VIRTUAL, 9 | }; 10 | 11 | 12 | enum GS_posType { 13 | ABSOLUTE, 14 | RELATIVE, 15 | }; 16 | 17 | #endif -------------------------------------------------------------------------------- /src/GyverPlanner.h: -------------------------------------------------------------------------------- 1 | /* 2 | Многоосевой планировщик траекторий для шаговых моторов 3 | - ОСТАНОВКА В КАЖДОЙ ТОЧКЕ. БУФЕР НА ОДНУ СЛЕДУЮЩУЮ ПОЗИЦИЮ 4 | - Макс. скорость: 5 | - Обычный режим: 37000 шаг/с на полной, 14000 шаг/с на разгоне 6 | - Быстрый профиль: 37000 шаг/с на полной, 37000 шаг/с на разгоне 7 | - Трапецеидальный профиль скорости (планировщик 2-го порядка) 8 | - Настройка скорости и ускорения 9 | - Любое количество осей. Будут двигаться синхронно к заданным целям 10 | - Быстрая целочисленная модель планирования траектории и скорости 11 | - Режим постоянного вращения для одной оси (для движения к концевику например) 12 | - Тормоз/плавная остановка/пауза на траектории планировщика 13 | - Оптимизировано для работы по прерыванию таймера 14 | - Быстрый контроль пинов шаговика для Arduino AVR 15 | 16 | AlexGyver, alex@alexgyver.ru 17 | https://alexgyver.ru/ 18 | MIT License 19 | */ 20 | 21 | /* 22 | GPlanner<драйвер, количество осей> planner; // объявяление 23 | 24 | void addStepper(uint8_t axis, Stepper &stp); // подключить мотор класса Stepper на ось axis 25 | // примечание: тип драйвера должен совпадать у планировщика и моторов 26 | 27 | void setBacklash(uint8_t axis, uint16_t steps); // установить компенсацию люфта на ось axis в количестве шагов steps 28 | void enable(); // включить моторы 29 | void disable(); // выключить моторы 30 | void power(bool v); // переключить питание 31 | 32 | // НАСТРОЙКИ 33 | void setMaxSpeed(float nV); // установка максимальной скорости планировщика в шаг/сек 34 | void setAcceleration(uint16_t nA); // установка ускорения планировщика в шаг/сек^2 35 | 36 | // ПЛАНИРОВЩИК 37 | uint32_t getPeriod(); // возвращает время в мкс до следующего вызова tick/tickManual 38 | bool ready(); // true - готов принять следующую точку маршрута 39 | void pause(); // пауза (доехать до заданной точки и ждать). ready() не вернёт true, пока ты на паузе 40 | void stop(); // остановить плавно (с заданным ускорением) 41 | void brake(); // резко остановить моторы из любого режима 42 | void resume(); // продолжить после остановки/паузы 43 | void reset(); // сбросить счётчики всех моторов в 0 44 | void home(); // отправить в 0 по всем осям 45 | uint8_t getStatus(); // текущий статус: 0 - стоим, 1 - едем, 2 - едем к точке паузы, 3 -крутимся со скоростью, 4 - тормозим 46 | 47 | // СКОРОСТЬ 48 | void setSpeed(uint8_t axis, float speed); // режим постоянного вращения для оси axis со скоростью speed шаг/сек (м.б. отрицателеьной) 49 | 50 | // ПОЗИЦИЯ 51 | void setCurrent(int16_t cur[]); // установить текущее положение моторов 52 | void setCurrent(int32_t cur[]); // установить текущее положение моторов 53 | int32_t getCurrent(int axis); // получить текущую позицию по оси axis 54 | 55 | // установить цель в шагах и начать движение. type - ABSOLUTE (по умолч.) или RELATIVE 56 | // ABSOLUTE - конкретные координаты точки, куда двигаться 57 | // RELATIVE - смещение относительно текущих положений моторов 58 | // вернёт true, если цель установлена. false, если цель совпадает с текущей 59 | bool setTarget(int32_t target[]); 60 | bool setTarget(int16_t target[]); 61 | bool setTarget(int32_t target[], type); 62 | bool setTarget(int16_t target[], type); 63 | int32_t getTarget(int axis); // получить цель в шагах на оси axis 64 | 65 | // ТИКЕР 66 | // тикер, вызывать как можно чаще. Вернёт true, если мотор крутится 67 | // здесь делаются шаги как для движения по точкам, так и для вращения по скорости 68 | bool tick(); 69 | 70 | // ручной тикер для вызова в прерывании или где то ещё. Выполняется 20..50 us 71 | bool tickManual(); 72 | 73 | // ОСОБЕННОСТИ 74 | - Планировщик не поддерживает горячую смену цели с плавным изменением скорости 75 | */ 76 | 77 | #ifndef _GyverPlanner_h 78 | #define _GyverPlanner_h 79 | #include 80 | 81 | #include "StepperCore.h" 82 | 83 | #define GP_MIN_US 300000 // период, длиннее которого мотор можно резко тормозить или менять скорость 84 | 85 | // создать планировщик с драйверами типа DRV и количеством осей AXLES 86 | template 87 | class GPlanner { 88 | public: 89 | GPlanner() { 90 | setAcceleration(100); 91 | setMaxSpeed(300); 92 | } 93 | 94 | // ============================== MOTOR ============================== 95 | // добавить объект типа Stepper на ось axis, начиная с 0 96 | void addStepper(uint8_t axis, Stepper<_DRV>& stp) { 97 | if (axis < _AXLES) steppers[axis] = &stp; 98 | } 99 | 100 | // установить компенсацию люфта на ось axis в количестве шагов steps 101 | void setBacklash(uint8_t axis, uint16_t steps) { 102 | blash[axis] = steps; 103 | } 104 | 105 | // ============================== POWER ============================== 106 | // включить моторы 107 | void enable() { 108 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->enable(); 109 | } 110 | 111 | // выключить моторы 112 | void disable() { 113 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->disable(); 114 | } 115 | 116 | // переключить питание 117 | void power(bool v) { 118 | if (v) enable(); 119 | else disable(); 120 | } 121 | 122 | // ============================= PLANNER ============================= 123 | // true - готов принять следующую точку маршрута 124 | bool ready() { 125 | if (readyF && !status) { 126 | readyF = false; 127 | return true; 128 | } 129 | return false; 130 | } 131 | 132 | // установка максимальной скорости планировщика в шаг/сек 133 | void setMaxSpeed(double speed) { 134 | nV = speed; 135 | if (!status) { 136 | V = nV; 137 | usMin = 1000000.0 / V; 138 | setAcceleration(na); 139 | changeSett = 0; 140 | } else changeSett = 1; 141 | } 142 | 143 | // установка ускорения планировщика в шаг/сек^2 144 | void setAcceleration(uint16_t acc) { 145 | na = acc; 146 | if (!status) { 147 | a = na; 148 | if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); 149 | else us0 = usMin; 150 | changeSett = 0; 151 | calcPlan(); 152 | } else changeSett = 1; 153 | } 154 | 155 | // пауза (доехать до заданной точки и ждать). ready() не вернёт true, пока ты на паузе 156 | void pause() { 157 | if (status == 1) status = 2; 158 | } 159 | 160 | // остановить плавно (с заданным ускорением) 161 | void stop() { 162 | // нет ускорения или крутим или медленно едем - дёргай ручник 163 | if (us == 0 || a == 0 || ((uint32_t)us << shift) > GP_MIN_US || status == 3 || !status) { 164 | brake(); 165 | return; 166 | } 167 | if (status <= 2) { // едем 168 | if (step > s2) { // а мы уже тормозим! 169 | pause(); // значит флаг на паузу 170 | return; 171 | } 172 | us <<= shift; 173 | stopStep = 1000000ul / us; // наша скорость 174 | stopStep = (uint32_t)stopStep * stopStep / 2 / a; // дистанция остановки. a не может быть 0 175 | us10 = (uint32_t)us << 10; 176 | us >>= shift; 177 | status = 4; 178 | } 179 | } 180 | 181 | // резко остановить моторы из любого режима 182 | void brake() { 183 | status = 0; 184 | us = 0; 185 | } 186 | 187 | // продолжить после остановки/паузы 188 | void resume() { 189 | setTarget(tar); 190 | } 191 | 192 | // ============================= POSITION ============================= 193 | // сбросить счётчики всех моторов в 0 194 | void reset() { 195 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = 0; 196 | } 197 | void home() { 198 | int32_t pos[_AXLES] = {}; 199 | setTarget(pos); 200 | } 201 | 202 | // установить текущее положение моторов 203 | void setCurrent(int16_t cur[]) { 204 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = (int32_t)cur[i]; 205 | } 206 | void setCurrent(int32_t cur[]) { 207 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = cur[i]; 208 | } 209 | 210 | // получить текущую позицию по оси axis 211 | int32_t getCurrent(int axis) { 212 | return steppers[axis]->pos; 213 | } 214 | 215 | // установить цель в шагах и начать движение. ~100 us 216 | bool setTarget(const int32_t target[], GS_posType type = ABSOLUTE) { 217 | if (changeSett) { // применяем настройки 218 | setMaxSpeed(nV); 219 | changeSett = 0; 220 | } 221 | 222 | S = 0; // путь 223 | for (uint8_t i = 0; i < _AXLES; i++) { // для всех осей 224 | tar[i] = target[i]; // запоминаем цель 225 | if (type == RELATIVE) tar[i] += steppers[i]->pos; // если относительное смещение - прибавляем текущий pos 226 | dS[i] = abs(tar[i] - steppers[i]->pos); // модуль ошибки по оси 227 | if (dS[i] > S) S = dS[i]; // ищем максимальное отклонение 228 | int8_t dir = (steppers[i]->pos < tar[i]) ? 1 : -1; // направление движения по оси 229 | if (blash[i] && steppers[i]->dir != dir) { // разворот! Учитываем люфт 230 | blash_buf[i] = blash[i]; 231 | } 232 | steppers[i]->dir = dir; 233 | } 234 | if (S == 0) { // путь == 0, мы никуда не едем 235 | readyF = true; // готовы к следующей точке 236 | brake(); // стоп машина 237 | return 0; 238 | } 239 | 240 | shift = 0; 241 | for (; shift < 5; shift++) { 242 | if ((uint32_t)usMin >> shift < 200 || (0xfffffffl >> shift) < S) break; 243 | } 244 | shift--; 245 | int32_t subS = ((int32_t)S << shift) >> 1; 246 | for (int i = 0; i < _AXLES; i++) nd[i] = subS; // записываем половину 247 | 248 | // расчёт точек смены характера движения 249 | // s1 - окончание разгона, s1-s2 - равномерное движение, s2 - торможение 250 | if (a > 0) { // ускорение задано 251 | us <<= shift; 252 | if (us != 0 && us < GP_MIN_US) { // мы движемся! ААА! 253 | int32_t v1 = 1000000L / us; 254 | if ((int32_t)V * V / a - ((int32_t)v1 * v1 / a >> 1) > S) { // треугольник 255 | s1 = ((int32_t)S >> 1) - ((int32_t)v1 * v1 / a >> 2); 256 | s2 = s1; 257 | } else { // трапеция 258 | s1 = ((int32_t)V * V - (int32_t)v1 * v1) / (2L * a); 259 | s2 = S - (int32_t)V * V / (2 * a); 260 | } 261 | so1 = (int32_t)v1 * v1 / (2 * a); 262 | } else { // не движемся 263 | if ((int32_t)V * V / a > S) { // треугольник 264 | s1 = S / 2L; 265 | s2 = s1; 266 | } else { // трапеция 267 | s1 = (int32_t)V * V / (2 * a); 268 | s2 = S - s1; 269 | } 270 | so1 = 0; 271 | us = us0; 272 | } 273 | } else { // ускорение отключено 274 | s1 = 0; 275 | s2 = S; 276 | us = usMin; 277 | } 278 | // здесь us10 - us*1024 для повышения разрешения микросекунд в 1024 раз 279 | us10 = (uint32_t)us << 10; 280 | us >>= shift; 281 | step = substep = 0; 282 | readyF = false; 283 | status = 1; 284 | return 1; 285 | } 286 | 287 | bool setTarget(const int16_t target[], GS_posType type = ABSOLUTE) { 288 | int32_t tar[_AXLES]; 289 | for (uint8_t i = 0; i < _AXLES; i++) tar[i] = target[i]; 290 | return setTarget(tar, type); 291 | } 292 | 293 | // получить цель в шагах на оси axis 294 | int32_t getTarget(int axis) { 295 | return tar[axis]; 296 | } 297 | 298 | // ============================= SPEED ============================ 299 | // режим постоянного вращения для оси axis со скоростью speed шаг/сек 300 | void setSpeed(uint8_t axis, float speed) { 301 | if (speed == 0) { // это куда ты собрался? 302 | brake(); 303 | return; 304 | } 305 | speedAxis = axis; // запомнили ось 306 | steppers[axis]->dir = speed > 0 ? 1 : -1; // направление 307 | us = 1000000.0 / abs(speed); // период 308 | us >>= shift; 309 | status = 3; 310 | } 311 | 312 | // ============================= TICK ============================= 313 | // тикер движения. Вернёт false если мотор остановлен. ~20..65us 314 | bool tick() { 315 | uint32_t now = micros(); 316 | if (status > 0 && now - tmr >= us) { 317 | tmr = now; //+= us;//= micros(); 318 | tickManual(); 319 | } 320 | return status; 321 | } 322 | 323 | bool tickManual() { 324 | // режим постоянной скорости 325 | if (status == 3) { 326 | steppers[speedAxis]->step(); 327 | return 1; 328 | } 329 | 330 | // выбираем люфт 331 | bool skip = 0; 332 | for (uint8_t i = 0; i < _AXLES; i++) { 333 | if (blash_buf[i]) { 334 | blash_buf[i]--; 335 | steppers[i]->step(); 336 | steppers[i]->pos += -steppers[i]->dir; 337 | if (!skip) skip = 1; 338 | } 339 | } 340 | if (skip) return 1; 341 | 342 | // здесь step - шаг вдоль общей линии траектории длиной S 343 | // шаги на проекциях получаются через алгоритм Брезенхема 344 | for (uint8_t i = 0; i < _AXLES; i++) { 345 | // http://members.chello.at/easyfilter/bresenham.html 346 | nd[i] -= dS[i]; 347 | if (nd[i] < 0) { 348 | nd[i] += (int32_t)S << shift; 349 | steppers[i]->step(); 350 | } 351 | } 352 | 353 | if (shift) 354 | if (++substep & ((1 << shift) - 1)) return status; // пропускаем сабшаги 355 | 356 | step++; 357 | // плавная остановка 358 | if (status == 4) { 359 | stopStep--; 360 | us10 += 2ul * us10 / (4ul * stopStep + 1); // торможение 361 | us = (uint32_t)us10 >> 10; 362 | us = constrain(us, usMin, us0); 363 | if (stopStep <= 0 || step >= S || us >= us0) brake(); 364 | us >>= shift; 365 | return status; 366 | } 367 | 368 | // https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/ 369 | // здесь us10 - us*1024 для повышения разрешения микросекунд 370 | if (step < s1) { // разгон 371 | #ifndef GS_FAST_PROFILE 372 | us10 -= 2ul * us10 / (4ul * (step + so1) + 1); 373 | us = (uint32_t)us10 >> 10; 374 | us = constrain(us, usMin, us0); 375 | #else 376 | if ((step + so1) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 377 | else { 378 | int j = 0; 379 | while ((step + so1) >= prfS[j]) j++; 380 | us = prfP[j]; 381 | } 382 | #endif 383 | } else if (step < s2) us = usMin; // постоянная 384 | else if (step < S) { // торможение 385 | #ifndef GS_FAST_PROFILE 386 | us10 += 2ul * us10 / (4ul * (S - step) + 1); 387 | us = (uint32_t)us10 >> 10; 388 | us = constrain(us, usMin, us0); 389 | #else 390 | if ((S - step) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 391 | else { 392 | int j = 0; 393 | while ((S - step) >= prfS[j]) j++; 394 | us = prfP[j]; 395 | } 396 | #endif 397 | } else { // приехали 398 | if (status == 1) readyF = true; 399 | brake(); 400 | } 401 | us >>= shift; 402 | return status; 403 | } 404 | 405 | uint32_t getPeriod() { 406 | return us << shift; 407 | } 408 | 409 | // текущий статус: 0 - стоим, 1 - едем, 2 - едем к точке паузы, 3 - крутимся со скоростью, 4 - тормозим 410 | uint8_t getStatus() { 411 | return status; 412 | } 413 | 414 | // ============================== PRIVATE ============================== 415 | private: 416 | void calcPlan() { 417 | #ifdef GS_FAST_PROFILE 418 | if (a > 0) { 419 | uint32_t sa = (uint32_t)V * V / a / 2ul; // расстояние разгона 420 | float dtf = sqrt(2.0 * sa / a) / GS_FAST_PROFILE; // время участка профиля 421 | float s0 = a * dtf * dtf / 2.0; // первый участок профиля 422 | uint32_t dt = dtf * 1000000.0; // время участка в секундах 423 | for (int i = 0; i < GS_FAST_PROFILE; i++) { 424 | prfS[i] = s0 * (i + 1) * (i + 1); 425 | uint32_t ds = prfS[i]; 426 | if (i > 0) ds -= prfS[i - 1]; 427 | if (ds <= 0) prfP[i] = 0; 428 | else prfP[i] = (uint32_t)dt / ds; 429 | } 430 | } 431 | #endif 432 | } 433 | 434 | #ifdef GS_FAST_PROFILE 435 | // массивы шагов и периодов для быстрого профиля скорости 436 | uint32_t prfS[GS_FAST_PROFILE], prfP[GS_FAST_PROFILE]; 437 | #endif 438 | 439 | uint16_t blash[_AXLES] = {}, blash_buf[_AXLES] = {}; 440 | 441 | uint32_t us; // период шагов 442 | int32_t tar[_AXLES], nd[_AXLES], dS[_AXLES]; // цель, переменная Брезенхема, смещение по оси 443 | int32_t step, substep, S, s1, s2, so1; // шаги на текущем участке, дробные шаги, длина участка, (s1,s2,so1) - для расчёта трапеций 444 | uint32_t tmr, us0, usMin, us10; // таймер тика, время первого шага, мин. период шага, сдвинутый на 10 us 445 | uint16_t a, na; // ускорение, буфер ускорения для применения после остановки 446 | int16_t stopStep; // шагов до остановки 447 | float V, nV; // скорость, буфер скорости для применения после остановки 448 | uint8_t status = 0, speedAxis = 0; // статус, ось в режиме скорости 449 | uint8_t shift = 0; // сдвиг для повышения разрешения Брезенхема 450 | bool readyF = true; 451 | bool changeSett = 0; // флаг, что изменились настройки 452 | Stepper<_DRV>* steppers[_AXLES]; 453 | }; 454 | #endif 455 | -------------------------------------------------------------------------------- /src/GyverPlanner2.h: -------------------------------------------------------------------------------- 1 | /* 2 | Многоосевой планировщик траекторий для шаговых моторов 3 | - ПЛАНИРОВАНИЕ СКОРОСТИ НА МАРШРУТЕ. НАСТРАИВАЕМЫЙ БУФЕР 4 | - Макс. скорость: 5 | - Обычный режим: 37000 шаг/с на полной, 14000 шаг/с на разгоне 6 | - Быстрый профиль: 37000 шаг/с на полной, 37000 шаг/с на разгоне 7 | - Трапецеидальный профиль скорости (планировщик 2-го порядка) 8 | - Настройка скорости и ускорения 9 | - Любое количество осей. Будут двигаться синхронно к заданным целям 10 | - Быстрая целочисленная модель планирования траектории и скорости 11 | - Режим постоянного вращения для одной оси (для движения к концевику например) 12 | - Тормоз/плавная остановка/пауза на траектории планировщика 13 | - Оптимизировано для работы по прерыванию таймера 14 | - Быстрый контроль пинов шаговика для Arduino AVR 15 | 16 | AlexGyver, alex@alexgyver.ru 17 | https://alexgyver.ru/ 18 | MIT License 19 | */ 20 | 21 | /* 22 | GPlanner<драйвер, количество осей> planner; // объявяление 23 | GPlanner<драйвер, количество осей, размер буфера> planner; // + размер буфера (по умолч. 32) 24 | 25 | void addStepper(uint8_t axis, Stepper &stp); // подключить мотор класса Stepper на ось axis 26 | // примечание: тип драйвера должен совпадать у планировщика и моторов 27 | 28 | void setBacklash(uint8_t axis, uint16_t steps); // установить компенсацию люфта на ось axis в количестве шагов steps 29 | void enable(); // включить моторы 30 | void disable(); // выключить моторы 31 | void power(bool v); // переключить питание 32 | 33 | // НАСТРОЙКИ 34 | void setMaxSpeed(float nV); // установка максимальной скорости планировщика в шаг/сек 35 | void setAcceleration(uint16_t nA); // установка ускорения планировщика в шаг/сек^2 36 | void setDtA(float newDta); // установить dt смены скорости в повороте, 0.0.. 1.0 по умолч. 0.3 37 | 38 | // ПЛАНИРОВЩИК 39 | uint32_t getPeriod(); // возвращает время в мкс до следующего вызова tick/tickManual 40 | void start(); // начать работу 41 | void stop(); // остановить плавно (с заданным ускорением) 42 | void brake(); // резко остановить моторы из любого режима 43 | void resume(); // продолжить после остановки или конечной точки маршрута 44 | void reset(); // сбросить счётчики всех моторов в 0 45 | bool ready(); // флаг достижения точки остановки. После неё нужно вызывать resume 46 | bool available(); // true - в буфере планировщика есть место под новю точку 47 | 48 | uint8_t getStatus(); // текущий статус: 49 | // 0 ожидание команды (остановлен) 50 | // 1 ожидание буфера 51 | // 2 в пути 52 | // 3 на паузу 53 | // 4 на стоп 54 | // 5 крутится setSpeed 55 | 56 | // СКОРОСТЬ 57 | void setSpeed(uint8_t axis, float speed); // режим постоянного вращения для оси axis со скоростью speed шаг/сек (м.б. отрицателеьной) 58 | 59 | // ПОЗИЦИЯ 60 | // добавить новую точку маршрута. Массив координат, флаг окончания и абсолютный/относительный 61 | void addTarget(int32_t tar[], uint8_t l, GS_posType type = ABSOLUTE); 62 | void addTarget(int16_t tar[], uint8_t l, GS_posType type = ABSOLUTE); 63 | // ABSOLUTE - конкретные координаты точки, куда двигаться 64 | // RELATIVE - смещение относительно текущих положений моторов 65 | 66 | void setCurrent(int16_t cur[]); // установить текущее положение моторов 67 | void setCurrent(int32_t cur[]); // установить текущее положение моторов 68 | int32_t getCurrent(int axis); // получить текущую позицию по оси axis 69 | int32_t getTarget(int axis); // получить текущую цель в шагах на оси axis 70 | 71 | // ТИКЕР 72 | // тикер, вызывать как можно чаще. Вернёт true, если мотор крутится 73 | // здесь делаются шаги для движения по точкам, для вращения по скорости, а также перестройка буфера 74 | bool tick(); 75 | 76 | // ручной тикер для вызова в прерывании или где то ещё. Выполняется 20..50 us 77 | bool tickManual(); 78 | 79 | // обработчик буфера. Сам вызывается в tick. Нужно вызывать вручную при работе с tickManual 80 | // вернёт true, если планировщик отправил моторы на новую позицию (в этот момент можно запускать таймер) 81 | void checkBuffer(); 82 | 83 | void clearBuffer(); // очистить буфер. Вызывать, когда планировщик остановлен! 84 | */ 85 | 86 | #ifndef _GyverPlanner2_h 87 | #define _GyverPlanner2_h 88 | #include 89 | 90 | #include "FIFO.h" 91 | #include "StepperCore.h" 92 | 93 | #define GP_MIN_US 300000 // период, длиннее которого мотор можно резко тормозить или менять скорость 94 | 95 | // создать планировщик с драйверами типа DRV и количеством осей AXLES 96 | template 97 | class GPlanner2 { 98 | public: 99 | GPlanner2() { 100 | setAcceleration(100); 101 | setMaxSpeed(300); 102 | } 103 | 104 | // ============================== MOTOR ============================== 105 | // добавить объект типа Stepper на ось axis, начиная с 0 106 | void addStepper(uint8_t axis, Stepper<_DRV>& stp) { 107 | if (axis < _AXLES) steppers[axis] = &stp; 108 | } 109 | 110 | // установить компенсацию люфта на ось axis в количестве шагов steps 111 | void setBacklash(uint8_t axis, uint16_t steps) { 112 | blash[axis] = steps; 113 | } 114 | 115 | // ============================= PLANNER ============================= 116 | // установка максимальной скорости планировщика в шаг/сек 117 | void setMaxSpeed(double speed) { 118 | nV = speed; 119 | if (!status) { 120 | V = nV; 121 | usMin = 1000000.0 / V; 122 | setAcceleration(na); 123 | changeSett = 0; 124 | } else changeSett = 1; 125 | } 126 | 127 | // установка ускорения планировщика в шаг/сек^2 128 | void setAcceleration(uint16_t acc) { 129 | na = acc; 130 | if (!status) { 131 | a = na; 132 | if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); 133 | else us0 = usMin; 134 | changeSett = 0; 135 | calcPlan(); 136 | } else changeSett = 1; 137 | } 138 | 139 | // установить dt смены скорости, 0.0.. 1.0 по умолч. 0.3 140 | void setDtA(float newDta) { 141 | dtA = newDta; 142 | } 143 | 144 | // остановить плавно (с заданным ускорением) 145 | void stop() { 146 | // мы и так уже остановились, успокойся 147 | if (((uint32_t)us << shift) == 0 || status == 0 || status == 4) return; 148 | // нет ускорения или медленно едем - дёргай ручник 149 | if (a == 0 || ((uint32_t)us << shift) > GP_MIN_US) { 150 | brake(); 151 | return; 152 | } 153 | us <<= shift; 154 | stopStep = 1000000ul / us; // наша скорость 155 | stopStep = (uint32_t)stopStep * stopStep / 2 / a; // дистанция остановки 156 | us10 = (uint32_t)us << 10; 157 | us >>= shift; 158 | status = 4; 159 | } 160 | 161 | // начать работу 162 | void start() { 163 | if (status == 0) status = 1; // если остановлен - проверяем буфер 164 | } 165 | 166 | // резко остановить моторы из любого режима 167 | void brake() { 168 | status = 0; 169 | us = 0; 170 | // пишем в буфер что мы остановились 171 | for (int i = 0; i < _AXLES; i++) bufP[i].set(0, steppers[i]->pos); 172 | bufV.set(0, 0); 173 | } 174 | 175 | // продолжить после остановки/паузы 176 | void resume() { 177 | if (status == 0) status = 1; // если стоим ждём - проверить буфер 178 | } 179 | 180 | // ============================== POWER ============================== 181 | // включить моторы 182 | void enable() { 183 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->enable(); 184 | } 185 | 186 | // выключить моторы 187 | void disable() { 188 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->disable(); 189 | } 190 | 191 | // переключить питание 192 | void power(bool v) { 193 | if (v) enable(); 194 | else disable(); 195 | } 196 | 197 | // ============================= POSITION ============================= 198 | // сбросить счётчики всех моторов в 0 199 | void reset() { 200 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = 0; 201 | } 202 | 203 | // установить текущее положение моторов 204 | void setCurrent(int16_t cur[]) { 205 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = (int32_t)cur[i]; 206 | } 207 | void setCurrent(int32_t cur[]) { 208 | for (uint8_t i = 0; i < _AXLES; i++) steppers[i]->pos = cur[i]; 209 | } 210 | 211 | // получить текущую позицию по оси axis 212 | int32_t getCurrent(int axis) { 213 | return steppers[axis]->pos; 214 | } 215 | 216 | // получить цель в шагах на оси axis 217 | int32_t getTarget(int axis) { 218 | return bufP[axis].get(1); 219 | } 220 | 221 | // ============================= SPEED ============================ 222 | // режим постоянного вращения для оси axis со скоростью speed шаг/сек 223 | void setSpeed(uint8_t axis, float speed) { 224 | if (speed == 0) { // это куда ты собрался? 225 | brake(); 226 | return; 227 | } 228 | speedAxis = axis; // запомнили ось 229 | steppers[axis]->dir = speed > 0 ? 1 : -1; // направление 230 | us = 1000000.0 / abs(speed); // период 231 | us >>= shift; 232 | status = 5; 233 | } 234 | 235 | // ============================= TICK ============================= 236 | // тикер движения. Вернёт false если мотор остановлен. ~20..65us + ~10мс при пересчёте блока 237 | bool tick() { 238 | checkBuffer(); 239 | uint32_t now = micros(); 240 | if (status > 1 && now - tmr >= us) { 241 | tmr = now; //+= us;//= micros(); 242 | tickManual(); 243 | } 244 | return status > 1; 245 | } 246 | 247 | bool tickManual() { 248 | // режим постоянной скорости 249 | if (status == 5) { 250 | steppers[speedAxis]->step(); 251 | return 1; 252 | } 253 | 254 | // выбираем люфт 255 | bool skip = 0; 256 | for (uint8_t i = 0; i < _AXLES; i++) { 257 | if (blash_buf[i]) { 258 | blash_buf[i]--; 259 | steppers[i]->step(); 260 | steppers[i]->pos += -steppers[i]->dir; 261 | if (!skip) skip = 1; 262 | } 263 | } 264 | if (skip) return 1; 265 | 266 | // здесь step - шаг вдоль общей линии траектории длиной S 267 | // шаги на проекциях получаются через алгоритм Брезенхема 268 | for (uint8_t i = 0; i < _AXLES; i++) { 269 | // http://members.chello.at/easyfilter/bresenham.html 270 | nd[i] -= dS[i]; 271 | if (nd[i] < 0) { 272 | nd[i] += (int32_t)S << shift; 273 | steppers[i]->step(); 274 | } 275 | } 276 | 277 | if (shift) 278 | if (++substep & ((1 << shift) - 1)) return status > 1; // пропускаем сабшаги 279 | step++; 280 | 281 | // плавная остановка 282 | if (status == 4) { 283 | stopStep--; 284 | us10 += 2ul * us10 / (4ul * stopStep + 1); // торможение 285 | us = (uint32_t)us10 >> 10; 286 | us = constrain(us, usMin, us0); 287 | if (step >= S) { 288 | next(); 289 | setTarget(); 290 | us = us10 >> 10; 291 | } 292 | if (stopStep <= 0 || us >= us0) brake(); 293 | us >>= shift; 294 | return status > 1; 295 | } 296 | 297 | // https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/ 298 | // здесь us10 - us*1024 для повышения разрешения микросекунд 299 | if (a > 0) { 300 | if (step < s1) { // разгон 301 | #ifndef GS_FAST_PROFILE 302 | us10 -= 2ul * us10 / (4ul * (step + so1) + 1); 303 | us = (uint32_t)us10 >> 10; 304 | us = constrain(us, usMin, us0); 305 | #else 306 | if ((step + so1) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 307 | else { 308 | int j = 0; 309 | while ((step + so1) >= prfS[j]) j++; 310 | us = prfP[j]; 311 | } 312 | #endif 313 | } else if (step < s2) us = usMin; // постоянная 314 | else if (step < S) { // торможение 315 | #ifndef GS_FAST_PROFILE 316 | us10 += 2ul * us10 / (4ul * (S - step + so2) + 1); 317 | us = (uint32_t)us10 >> 10; 318 | us = constrain(us, usMin, us0); 319 | #else 320 | if ((S - step + so2) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 321 | else { 322 | int j = 0; 323 | while ((S - step + so2) >= prfS[j]) j++; 324 | us = prfP[j]; 325 | } 326 | #endif 327 | } 328 | } 329 | if (step >= S) { // приехали 330 | if (status == 3) { // достигли конечной точки 331 | readyF = true; 332 | status = 0; 333 | } else status = 1; // иначе проверяем буфер 334 | next(); 335 | } 336 | us >>= shift; 337 | return (status > 1); 338 | } 339 | 340 | // обработчик буфера. Сам вызывается в tick. Нужно вызывать вручную при работе с tickManual 341 | bool checkBuffer() { 342 | if (status == 1) { // достигли цели, запрашиваем новую 343 | if (bufL.available() > 1) { 344 | if (--block < 0) block = 0; 345 | // в движении с ускорением пересчитываем путь, если остановились или есть на это время 346 | if (a > 0 && (bufV.get(0) == 0 || (block == 0 && !changeSett && us > blockCalc))) calculateBlock(); 347 | if (setTarget()) return 1; 348 | } 349 | } 350 | return 0; 351 | } 352 | 353 | // флаг достижения точки остановки. После неё нужно вызывать resume 354 | bool ready() { 355 | if (readyF && !status) { 356 | readyF = false; 357 | return 1; 358 | } 359 | return 0; 360 | } 361 | 362 | // true - в буфере планировщика есть место под новю точку 363 | bool available() { 364 | return bufV.availableForWrite(); 365 | } 366 | 367 | // добавить новую точку. Массив координат, флаг окончания и абсолютный/относительный 368 | void addTarget(int32_t tar[], uint8_t l, GS_posType type = ABSOLUTE) { 369 | if (type == ABSOLUTE) 370 | for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i]); 371 | else 372 | for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i] + bufP[i].getLast()); 373 | bufL.add(l); 374 | bufV.add(0); 375 | bufS.add(0); 376 | } 377 | 378 | void addTarget(int16_t tar[], uint8_t l, GS_posType type = ABSOLUTE) { 379 | int32_t ntar[_AXLES]; 380 | for (int i = 0; i < _AXLES; i++) ntar[i] = (int32_t)tar[i]; 381 | addTarget(ntar, l, type); 382 | } 383 | 384 | // время до следующего tick, мкс 385 | uint32_t getPeriod() { 386 | return us << shift; 387 | } 388 | 389 | // статус планировщика 390 | uint8_t getStatus() { 391 | return status; 392 | } 393 | 394 | // очистить буфер 395 | void clearBuffer() { 396 | for (int i = 0; i < _AXLES; i++) bufP[i].clear(); 397 | bufL.clear(); 398 | bufV.clear(); 399 | bufS.clear(); 400 | } 401 | 402 | // ============================== PRIVATE ============================== 403 | private: 404 | // пересчитать скорости 405 | void calculateBlock() { 406 | if (changeSett && bufV.get(0) == 0) { // параметры движения изменились, а мы стоим 407 | noInterrupts(); 408 | uint8_t stBuf = status; 409 | status = 0; 410 | setMaxSpeed(nV); 411 | status = stBuf; 412 | interrupts(); 413 | changeSett = 0; 414 | } 415 | 416 | uint32_t calcTime = micros(); 417 | block = _BUF / 2; // через половину буфера попробуем сделать перерасчёт пути 418 | uint32_t nextS = calcS(0); 419 | 420 | // поиск максимальной конечной скорости 421 | for (uint16_t i = 0; i < bufV.available() - 1; i++) { 422 | int32_t dn0[_AXLES]; 423 | for (int j = 0; j < _AXLES; j++) dn0[j] = bufP[j].get(i + 1) - bufP[j].get(i); // расстояние между точками (катеты) 424 | uint32_t S1 = nextS; // "гипотенуза" (на 1 шаге посчитана выше) 425 | bufS.set(i, S1); // записали в буфер 426 | if (bufL.get(i + 1) == 1) break; // последняя точка - выходим, потом посчитаем 427 | if (S1 == 0) continue; // гипотенуза 0 - пропускаем 428 | 429 | if (i < bufV.available() - 2) { // для следующих точек (+1) 430 | int32_t multSum = 0; 431 | // складываем перемноженные катеты для расчёта косинуса 432 | for (int j = 0; j < _AXLES; j++) multSum += (int32_t)dn0[j] * (bufP[j].get(i + 2) - bufP[j].get(i + 1)); 433 | nextS = calcS(i + 1); // гипотенуза 434 | if (nextS == 0) continue; // гипотенуза 0 - пропускаем 435 | float cosa = -(float)multSum / S1 / nextS; // косинус угла между отрезками пути 436 | float Vm; // макс. скорость в повороте 437 | if (cosa < -0.95) Vm = V; // пролетаем по прямой 438 | else { 439 | Vm = (float)a * dtA / sqrt(2.0 * (1 + cosa)); // считаем 440 | Vm = min(Vm, V); // на всякий случай ограничим 441 | } 442 | bufV.set(i + 1, Vm); // пишем в буфер 443 | } 444 | } 445 | 446 | // уменьшаем переходные скорости на траектории 447 | for (uint16_t i = 0; i < bufV.available() - 1; i++) { 448 | uint32_t v0 = bufV.get(i); 449 | uint32_t v1 = bufV.get(i + 1); 450 | uint32_t maxV = sqrt(2L * a * bufS.get(i) + (uint32_t)v0 * v0); 451 | if (v1 > v0 && maxV < v1) bufV.set(i + 1, maxV); // всё в порядке 452 | else if (v1 < v0 && maxV > v1) { // идём назад по буферу и уменьшаем скорости 453 | int16_t count = 0; 454 | while (true) { 455 | uint32_t minV = bufV.get(i + count + 1); 456 | minV *= (uint32_t)minV; 457 | minV = sqrt(2ul * a * bufS.get(i + count) + minV); 458 | if (minV >= bufV.get(i + count)) break; 459 | else bufV.set(i + count, minV); 460 | count--; 461 | } 462 | } 463 | } 464 | // находим среднее время пересчёта, чтобы вызвать его между точками (когда появится возможность) 465 | blockCalc = (blockCalc + micros() - calcTime) / 2; 466 | } 467 | 468 | void next() { 469 | for (int i = 0; i < _AXLES; i++) bufP[i].next(); 470 | bufL.next(); 471 | bufV.next(FIFO_WIPE); // обнуляем использованную ячейку 472 | bufS.next(); 473 | } 474 | 475 | // установить цель в шагах и начать движение. ~100 us 476 | bool setTarget() { 477 | tmr = micros(); 478 | for (uint8_t i = 0; i < _AXLES; i++) { // для всех осей 479 | dS[i] = abs(bufP[i].get(1) - steppers[i]->pos); // модуль ошибки по оси 480 | int8_t dir = steppers[i]->pos < bufP[i].get(1) ? 1 : -1; // направление движения по оси 481 | if (blash[i] && steppers[i]->dir != dir) { // разворот! Учитываем люфт 482 | blash_buf[i] = blash[i]; 483 | } 484 | steppers[i]->dir = dir; 485 | } 486 | 487 | if (a == 0) S = calcS(0); 488 | else S = bufS.get(0); 489 | 490 | if (S == 0) { // путь == 0, мы никуда не едем 491 | status = 1; // на буфер 492 | next(); 493 | return 0; 494 | } 495 | 496 | shift = 0; 497 | for (; shift < 5; shift++) { 498 | if ((uint32_t)usMin >> shift < 200 || (0xfffffffl >> shift) < S) break; 499 | } 500 | shift--; 501 | int32_t subS = ((int32_t)S << shift) >> 1; 502 | for (int i = 0; i < _AXLES; i++) nd[i] = subS; // записываем половину 503 | 504 | if (a > 0) { 505 | int32_t v1 = bufV.get(0); // скорость начала отрезка 506 | int32_t v2 = bufV.get(1); // скорость конца отрезка 507 | 508 | if ((V * V - ((int32_t)v1 * v1 >> 1) - ((int32_t)v2 * v2 >> 1)) / a > S) { // треугольник 509 | s1 = ((int32_t)S >> 1) + (((int32_t)v2 * v2 >> 2) - ((int32_t)v1 * v1 >> 2)) / a; 510 | s2 = 0; 511 | } else { // трапеция 512 | s1 = ((int32_t)V * V - (int32_t)v1 * v1) / (2L * a); 513 | s2 = S - ((int32_t)V * V - (int32_t)v2 * v2) / (2L * a); 514 | } 515 | so1 = (int32_t)v1 * v1 / (2L * a); 516 | so2 = (int32_t)v2 * v2 / (2L * a); 517 | if (status != 4) { 518 | if (v1 == 0) us = us0; 519 | else us = 1000000ul / v1; 520 | } 521 | } else { 522 | s1 = 0; 523 | s2 = S; 524 | so1 = so2 = 0; 525 | us = usMin; 526 | } 527 | 528 | step = substep = 0; 529 | readyF = false; 530 | if (status != 4) { // если это не стоп 531 | if (bufL.get(1) == 1) status = 3; 532 | else status = 2; 533 | us10 = us << 10; 534 | us >>= shift; 535 | } 536 | return 1; 537 | } 538 | 539 | uint32_t calcS(int i) { 540 | uint32_t sqSum = 0; 541 | int32_t dj; 542 | for (int j = 0; j < _AXLES; j++) { 543 | dj = bufP[j].get(i + 1) - bufP[j].get(i); 544 | sqSum += (int32_t)dj * dj; 545 | } 546 | return sqrt(sqSum); 547 | } 548 | 549 | void calcPlan() { 550 | #ifdef GS_FAST_PROFILE 551 | if (a > 0) { 552 | uint32_t sa = (uint32_t)V * V / a / 2ul; // расстояние разгона 553 | float dtf = sqrt(2.0 * sa / a) / GS_FAST_PROFILE; // время участка профиля 554 | float s0 = a * dtf * dtf / 2.0; // первый участок профиля 555 | uint32_t dt = dtf * 1000000.0; // время участка в секундах 556 | for (int i = 0; i < GS_FAST_PROFILE; i++) { 557 | prfS[i] = s0 * (i + 1) * (i + 1); 558 | uint32_t ds = prfS[i]; 559 | if (i > 0) ds -= prfS[i - 1]; 560 | if (ds <= 0) prfP[i] = 0; 561 | else prfP[i] = (uint32_t)dt / ds; 562 | } 563 | } 564 | #endif 565 | } 566 | 567 | #ifdef GS_FAST_PROFILE 568 | uint32_t prfS[GS_FAST_PROFILE], prfP[GS_FAST_PROFILE]; 569 | #endif 570 | 571 | uint16_t blash[_AXLES] = {}, blash_buf[_AXLES] = {}; 572 | 573 | uint32_t us; 574 | int32_t nd[_AXLES], dS[_AXLES]; 575 | int32_t step, substep, S, s1, s2, so1, so2; 576 | uint32_t tmr, us0, usMin, us10; 577 | uint16_t a, na; 578 | int16_t stopStep; 579 | float V, nV; 580 | uint8_t status = 0, speedAxis = 0, maxAx; 581 | uint8_t shift = 0; 582 | bool readyF = true; 583 | float dtA = 0.3; 584 | 585 | int8_t block = 100; 586 | uint32_t blockCalc = 20000; 587 | bool changeSett = 0; 588 | 589 | Stepper<_DRV>* steppers[_AXLES]; 590 | FIFO bufP[_AXLES]; 591 | FIFO bufL; 592 | FIFO bufV; 593 | FIFO bufS; 594 | }; 595 | 596 | #endif -------------------------------------------------------------------------------- /src/GyverStepper.h: -------------------------------------------------------------------------------- 1 | /* 2 | Производительная библиотека для управления шаговыми моторами с Arduino 3 | Документация: https://alexgyver.ru/gyverstepper/ 4 | GitHub: https://github.com/GyverLibs/GyverStepper 5 | Возможности: 6 | - Поддержка 4х пинового (шаг и полушаг) и STEP-DIR драйверов 7 | - Автоматическое отключение питания при достижении цели 8 | - Режимы работы: 9 | - Вращение с заданной скоростью. Плавный разгон и торможение с ускорением 10 | - Следование к позиции с ускорением и ограничением скорости 11 | - Следование к позиции с заданной скоростью (без ускорения) 12 | - Быстрый алгоритм управления шагами 13 | - Два алгоритма плавного движения 14 | - Мой планировщик обеспечивает максимальную производительность: 15 | скорость до 30'000 шагов/сек с ускорением (активен по умолчанию) 16 | - Модифицированный планировщик из AccelStepper: максимальную плавность и 17 | скорость до 7'000 шагов/сек с ускорением (для активации пропиши дефайн SMOOTH_ALGORITHM) 18 | - Поддержка "виртуальных" драйверов 19 | 20 | Алгоритм из AccelStepper: https://www.airspayce.com/mikem/arduino/AccelStepper/ 21 | AlexGyver, alex@alexgyver.ru 22 | https://alexgyver.ru/ 23 | MIT License 24 | 25 | Версии: 26 | v1.1 - добавлена возможность плавного управления скоростью в KEEP_SPEED (см. пример accelDeccelButton) 27 | v1.2 - добавлена поддержка ESP8266 28 | v1.3 - изменена логика работы setTarget(, RELATIVE) 29 | v1.4 - добавлена задержка для STEP, настроить можно дефайном DRIVER_STEP_TIME 30 | v1.5 - пофикшен баг для плат есп 31 | v1.6 - Исправлена остановка для STEPPER4WIRE_HALF, скорость можно задавать во float (для медленных скоростей) 32 | v1.7 - Исправлен баг в отрицательной скорости (спасибо Евгению Солодову) 33 | v1.8 - Исправлен режим KEEP_SPEED 34 | v1.9 - Исправлена ошибка с esp функцией max 35 | v1.10 - повышена точность 36 | v1.11 - повышена точность задания скорости 37 | v1.12 - пофикшена плавная работа в KEEP_SPEED. Добавлена поддержка "внешних" драйверов. Убран аргумент SMOOTH из setSpeed 38 | v1.13 - исправлены мелкие баги, оптимизация 39 | v1.14 - исправлены ошибки разгона и торможения в KEEP_SPEED 40 | v1.15 - оптимизация, исправлены мелкие баги, stop() больше не сбрасывает maxSpeed 41 | v1.15.2 - добавил включение EN если указан, даже при отключенном autoPower 42 | v2.0 - оптимизация. Ядро шаговика вынесено в отдельный класс Stepper. Добавлены многоосевые планировщики траекторий 43 | v2.1 - добавил GyverStepper2, упрощённая и оптимизированная версия GyverStepper 44 | v2.1.1 - исправлена бага в GyverStepper 45 | v2.1.2 - совместимость Digispark 46 | v2.1.3 - починил FOLLOW_POS в GStepper, починил RELATIVE в GPlanner2 и исправил багу с рывками 47 | v2.1.4 - GPlanner2: исправил рывки, добавил адаптивное перестроение траектории без остановок, чутка оптимизировал вычисления 48 | v2.1.5 - возможность менять скорость и ускорение во время работы планировщика (GStepper2, GPlanner, GPlanner2) 49 | v2.1.6 - исправлена ошибка компиляции при вызове disable() в GStepper 50 | v2.1.7 - добавлен clearBuffer() в GPlanner2 51 | v2.1.8 - оптимизация, исправлен KEEP_SPEED в GStepper 52 | v2.2.0 - добавлен скоростной профиль GS_FAST_PROFILE для GStepper2, GPlanner, GPlanner2. Поддержка режима "слежения" для GStepper2 53 | v2.2.1 - небольшая оптимизация SRAM 54 | v2.3 - fix compiler warnings, поддержка esp32 55 | v2.4 - повышена плавность движения шаговиков в Planner и Planner2. Исправлена бага в Stepper2 56 | v2.5 - исправлено плавное изменение скорости для KEEP_SPEED 57 | v2.6 58 | - disable() в виртуальном режиме отключает сигнал с мотора (для 4-проводных драйверов) 59 | - улучшена производительность для step-dir драйверов 60 | - добавил autoPower() в GStepper2 61 | - исправлен рывок при смене направления в GStepper 62 | v2.6.1 - поправлена бага в GStepper2 63 | v2.6.2 - оптимизированы вычисления в GStepper2, GPlanner и GPlanner2 64 | v2.6.3 - reverse() в step-dir драйвере теперь применяется сразу 65 | */ 66 | 67 | /* 68 | // Примечание: далее по тексту под "по умолчанию" имеется в виду "даже если не вызывать функцию" 69 | 70 | // Создание объекта 71 | // steps - шагов на один оборот вала (для расчётов с градусами) 72 | // step, dir, pin1, pin2, pin3, pin4 - любые GPIO 73 | // en - пин отключения драйвера, любой GPIO 74 | GStepper stepper(steps, step, dir); // драйвер step-dir 75 | GStepper stepper(steps, step, dir, en); // драйвер step-dir + пин enable 76 | GStepper stepper(steps, pin1, pin2, pin3, pin4); // драйвер 4 пин 77 | GStepper stepper(steps, pin1, pin2, pin3, pin4, en); // драйвер 4 пин + enable 78 | GStepper stepper(steps, pin1, pin2, pin3, pin4); // драйвер 4 пин полушаг 79 | GStepper stepper(steps, pin1, pin2, pin3, pin4, en); // драйвер 4 пин полушаг + enable 80 | 81 | GStepper stepper(steps); // виртуальный драйвер step-dir 82 | GStepper stepper(steps); // виртуальный драйвер 4 пин 83 | 84 | // Здесь происходит движение мотора, вызывать как можно чаще! 85 | // Имеет встроенный таймер 86 | // Возвращает true, если мотор движется к цели или крутится по KEEP_SPEED 87 | bool tick(); 88 | 89 | // Инвертировать направление мотора - true (по умолч. false) 90 | void reverse(bool dir); 91 | 92 | // инвертировать поведение EN пина - true (по умолч. false) 93 | void invertEn(bool rev); 94 | 95 | // Установка режима работы, mode: 96 | // FOLLOW_POS - следование к позиции setTarget(...) 97 | // KEEP_SPEED - удержание скорости setSpeed(...) 98 | void setRunMode(GS_runMode mode); 99 | 100 | // Установка текущей позиции мотора в шагах и градусах 101 | void setCurrent(int32_t pos); 102 | void setCurrentDeg(float pos); 103 | 104 | // Чтение текущей позиции мотора в шагах и градусах 105 | int32_t getCurrent(); 106 | float getCurrentDeg(); 107 | 108 | // установка целевой позиции в шагах и градусах (для режима FOLLOW_POS) 109 | // type - ABSOLUTE или RELATIVE, по умолчанию стоит ABSOLUTE 110 | // RELATIVE считается от текущей позиции мотора 111 | void setTarget(int32_t pos); 112 | void setTarget(int32_t pos, GS_posType type); 113 | void setTargetDeg(float pos); 114 | void setTargetDeg(float pos, GS_posType type); 115 | 116 | // Получение целевой позиции в шагах и градусах 117 | int32_t getTarget(); 118 | float getTargetDeg(); 119 | 120 | // Установка максимальной скорости (по модулю) в шагах/секунду и градусах/секунду (для режима FOLLOW_POS) 121 | // по умолч. 300 122 | // минимум - 1 шаг в час 123 | void setMaxSpeed(float speed); 124 | void setMaxSpeedDeg(float speed); 125 | 126 | // Установка ускорения в шагах и градусах в секунду (для режима FOLLOW_POS). 127 | // При значении 0 ускорение отключается и мотор работает 128 | // по профилю постоянной максимальной скорости setMaxSpeed(). 129 | // По умолч. 300 130 | void setAcceleration(int accel); 131 | void setAccelerationDeg(float accel); 132 | 133 | // Автоотключение EN при достижении позиции - true (по умолч. false). 134 | void autoPower(bool mode); 135 | 136 | // Плавная остановка с заданным ускорением от текущего положения 137 | // Работает также в режиме KEEP_SPEED 138 | void stop(); 139 | 140 | // Жёсткая остановка. Отключает мотор, если включен autoPower 141 | void brake(); 142 | 143 | // Жёсткая остановка + сброс позиции в 0 (для концевиков) 144 | void reset(); 145 | 146 | // Установка целевой скорости в шагах/секунду и градусах/секунду (для режима KEEP_SPEED) 147 | // при ненулевом setAcceleration будет выполнен плавный разгон/торможение к нужной скорости 148 | // минимальная скорость - 1 шаг в час 149 | void setSpeed(float speed); 150 | void setSpeedDeg(float speed); 151 | 152 | // Получение целевой скорости в шагах/секунду и градусах/секунду (для режима KEEP_SPEED) 153 | float getSpeed(); 154 | float getSpeedDeg(); 155 | 156 | // Включить мотор (пин EN) 157 | void enable(); 158 | 159 | // Выключить мотор (пин EN) 160 | void disable(); 161 | 162 | // Возвращает то же самое, что tick, т.е. крутится мотор или нет 163 | bool getState(); 164 | 165 | // Возвращает минимальный период тика мотора в микросекундах при настроенной setMaxSpeed() скорости. 166 | // Можно использовать для настройки прерываний таймера, в обработчике которого будет лежать tick() (см. пример timerISR) 167 | uint32_t getMinPeriod(); 168 | 169 | // Текущий период "тика" для отладки и всего такого 170 | uint32_t stepTime; 171 | 172 | // подключить внешний обработчик для шага и переключения питания 173 | void attachStep(handler) 174 | void attachPower(handler) 175 | 176 | */ 177 | 178 | // Раскомментируй для использования более плавного, но медленного алгоритма 179 | // Также дефайн можно прописать в скетче до подключения библиотеки!!! См. пример smoothAlgorithm 180 | // #define SMOOTH_ALGORITHM 181 | 182 | #ifndef _GyverStepper_h 183 | #define _GyverStepper_h 184 | #include 185 | 186 | #include "GStypes.h" 187 | #include "StepperCore.h" 188 | 189 | // =========== МАКРОСЫ =========== 190 | #define degPerMinute(x) ((x) / 60.0f) 191 | #define degPerHour(x) ((x) / 3600.0f) 192 | #define _sign(x) ((x) >= 0 ? 1 : -1) // знак числа 193 | 194 | // =========== КОНСТАНТЫ =========== 195 | #define _MIN_SPEED_FP 5 // мин. скорость для движения в FOLLOW_POS с ускорением 196 | #define _MAX_PERIOD_FP (1000000L / _MIN_SPEED_FP) 197 | #define _MIN_STEP_SPEED (1.0 / 3600) // мин. скорость 1 шаг в час 198 | 199 | enum GS_runMode { 200 | FOLLOW_POS, 201 | KEEP_SPEED, 202 | }; 203 | 204 | enum GS_smoothType { 205 | NO_SMOOTH, 206 | SMOOTH, 207 | }; 208 | 209 | // ============================== GS CLASS ============================== 210 | template 211 | class GStepper : public Stepper<_DRV, _TYPE> { 212 | public: 213 | // конструктор 214 | GStepper(int stepsPerRev, uint8_t pin1 = 255, uint8_t pin2 = 255, uint8_t pin3 = 255, uint8_t pin4 = 255, uint8_t pin5 = 255) : Stepper<_DRV, _TYPE>(pin1, pin2, pin3, pin4, pin5) { 215 | // умолчания 216 | setMaxSpeed(300); 217 | setAcceleration(300); 218 | _stepsPerDeg = (stepsPerRev / 360.0); 219 | } 220 | 221 | // ============================== TICK ============================== 222 | // тикер, вызывать почаще. Возвращает true, если мотор всё ещё движется к цели 223 | bool tick() { 224 | if (_workState) { 225 | tickUs = micros(); 226 | #ifndef SMOOTH_ALGORITHM 227 | if (!_curMode && _accel != 0 && _maxSpeed >= _MIN_SPEED_FP) planner(); // планировщик скорости FOLLOW_POS быстрый 228 | #endif 229 | 230 | if (_curMode && _accel != 0) smoothSpeedPlanner(); // планировщик скорости KEEP_SPEED 231 | 232 | if (stepTime && tickUs - _prevTime >= stepTime) { // основной таймер степпера 233 | _prevTime = tickUs; 234 | 235 | #ifdef SMOOTH_ALGORITHM 236 | // плавный планировщик вызывается каждый шаг. Проверка остановки 237 | if (!_curMode && _accel != 0 && _maxSpeed >= _MIN_SPEED_FP && !plannerSmooth()) { 238 | brake(); 239 | return false; 240 | } 241 | #endif 242 | 243 | // проверка остановки для быстрого планировщика, а также работы без ускорения 244 | if (!_curMode && _target == pos) { 245 | brake(); 246 | return false; 247 | } 248 | step(); // двигаем мотор 249 | } 250 | } 251 | return _workState; 252 | } 253 | 254 | // ============================== SETTINGS ============================== 255 | 256 | // установка текущей позиции в шагах 257 | void setCurrent(int32_t npos) { 258 | pos = npos; 259 | _accelSpeed = 0; 260 | } 261 | 262 | // установка текущей позиции в градусах 263 | void setCurrentDeg(float npos) { 264 | setCurrent((float)npos * _stepsPerDeg); 265 | } 266 | 267 | // чтение текущей позиции в шагах 268 | int32_t getCurrent() { 269 | return pos; 270 | } 271 | 272 | // чтение текущей позиции в градусах 273 | float getCurrentDeg() { 274 | return ((float)pos / _stepsPerDeg); 275 | } 276 | 277 | // установка целевой позиции в шагах 278 | void setTarget(int32_t npos, GS_posType type = ABSOLUTE) { 279 | _target = type ? (npos + pos) : npos; 280 | if (_target != pos) { 281 | if (_accel == 0 || _maxSpeed < _MIN_SPEED_FP) { 282 | stepTime = 1000000.0 / _maxSpeed; 283 | dir = (_target > pos) ? 1 : -1; 284 | } 285 | enable(); 286 | } 287 | } 288 | 289 | // установка целевой позиции в градусах 290 | void setTargetDeg(double npos, GS_posType type = ABSOLUTE) { 291 | setTarget((float)npos * _stepsPerDeg, type); 292 | } 293 | 294 | // получение целевой позиции в шагах 295 | int32_t getTarget() { 296 | return _target; 297 | } 298 | 299 | // целевой позиции в градусах 300 | float getTargetDeg() { 301 | return ((float)_target / _stepsPerDeg); 302 | } 303 | 304 | // установка максимальной скорости в шагах/секунду 305 | void setMaxSpeed(double speed) { 306 | speed = abs(speed); 307 | _maxSpeed = max(speed, _MIN_STEP_SPEED); // 1 шаг в час минимум 308 | // считаем stepTime для низких скоростей или отключенного ускорения 309 | if (_accel == 0 || _maxSpeed < _MIN_SPEED_FP) stepTime = 1000000.0 / _maxSpeed; 310 | 311 | #ifdef SMOOTH_ALGORITHM 312 | _cmin = 1000000.0 / _maxSpeed; 313 | if (_n > 0) { 314 | _n = (float)_accelSpeed * _accelSpeed * _accelInv; 315 | plannerSmooth(); 316 | } 317 | #else 318 | // период планировщка в зависимости от макс. скорости 319 | _plannerPrd = map((int)_maxSpeed, 1000, 20000, 15000, 1000); 320 | _plannerPrd = constrain(_plannerPrd, 15000, 1000); 321 | #endif 322 | } 323 | 324 | // установка максимальной скорости в градусах/секунду 325 | void setMaxSpeedDeg(double speed) { 326 | setMaxSpeed(speed * _stepsPerDeg); 327 | } 328 | 329 | // установка ускорения в шагах/секунду^2 330 | void setAcceleration(uint16_t accel) { 331 | _accel = accel; 332 | if (_accel) _accelInv = 0.5f / accel; 333 | else _accelInv = 0; 334 | _accelTime = accel / 1000000.0f; 335 | #ifdef SMOOTH_ALGORITHM 336 | if (_accel) _c0 = 0.676 * sqrt(2.0 / _accel) * 1000000.0; 337 | plannerSmooth(); 338 | #endif 339 | } 340 | 341 | // установка ускорения в градусах/секунду^2 342 | void setAccelerationDeg(float accel) { 343 | setAcceleration(accel * _stepsPerDeg); 344 | } 345 | 346 | // автоотключение питания при остановке 347 | void autoPower(bool mode) { 348 | _autoPower = mode; 349 | } 350 | 351 | // плавная остановка с заданным ускорением 352 | void stop() { 353 | if (_workState) { 354 | resetTimers(); 355 | if (_curMode == FOLLOW_POS) { 356 | if (!_accel) { 357 | brake(); 358 | return; 359 | } 360 | _accelSpeed = 1000000.0f / stepTime * dir; 361 | setTarget(pos + (float)_accelSpeed * _accelSpeed * _accelInv * dir); 362 | // setMaxSpeed(abs(_accelSpeed)); 363 | _stopSpeed = abs(_accelSpeed); 364 | #ifdef SMOOTH_ALGORITHM 365 | _n = (float)_accelSpeed * _accelSpeed * _accelInv; 366 | #endif 367 | } else { 368 | setSpeed(0); 369 | } 370 | } 371 | } 372 | 373 | // остановка и сброс позиции в 0 374 | void reset() { 375 | brake(); 376 | setCurrent(0); 377 | } 378 | 379 | // установка целевой скорости в шагах/секунду 380 | void setSpeed(float speed, bool smooth = false) { // smooth убран! 381 | // 1 шаг в час минимум 382 | _speed = speed; 383 | _stopF = (_speed == 0); 384 | if (_speed == 0 && _accelSpeed == 0) return; 385 | dir = (_speed > 0) ? 1 : -1; 386 | if (abs(_speed) < _MIN_STEP_SPEED) _speed = _MIN_STEP_SPEED * dir; 387 | 388 | if (_accel != 0) { // плавный старт 389 | if (_accelSpeed != _speed) { 390 | int speed1 = (int)abs(_speed); 391 | int speed2 = (int)abs(_accelSpeed); 392 | _speedPlannerPrd = map(max(speed1, speed2), 1000, 20000, 15000, 2000); 393 | _speedPlannerPrd = constrain(_speedPlannerPrd, 15000, 2000); 394 | stepTime = abs(1000000.0 / _accelSpeed); 395 | } 396 | } else { // резкий старт 397 | if (speed == 0) { // скорость 0? Отключаемся и выходим 398 | brake(); 399 | return; 400 | } 401 | _accelSpeed = _speed; 402 | stepTime = abs(1000000.0 / _speed); 403 | } 404 | enable(); 405 | } 406 | 407 | // установка целевой скорости в градусах/секунду 408 | void setSpeedDeg(float speed, bool smooth = false) { 409 | setSpeed(_stepsPerDeg * speed); 410 | } 411 | 412 | // получение целевой скорости в шагах/секунду 413 | float getSpeed() { 414 | return (1000000.0 / stepTime * dir); 415 | } 416 | 417 | // получение целевой скорости в градусах/секунду 418 | float getSpeedDeg() { 419 | return ((float)getSpeed() / _stepsPerDeg); 420 | } 421 | 422 | // установка режима работы 423 | void setRunMode(GS_runMode mode) { 424 | if (_curMode != mode) resetTimers(); 425 | _curMode = mode; 426 | } 427 | 428 | // получить статус вкл/выкл 429 | bool getState() { 430 | return _workState; 431 | } 432 | 433 | // включить мотор 434 | void enable() { 435 | _workState = true; 436 | _stopSpeed = 0; 437 | resetTimers(); 438 | Stepper<_DRV, _TYPE>::enable(); 439 | } 440 | 441 | // резкая остановка 442 | void brake() { 443 | _workState = false; 444 | _stopSpeed = 0; 445 | resetMotor(); 446 | if (_autoPower) Stepper<_DRV, _TYPE>::disable(); 447 | } 448 | 449 | // получить минимальный период, с которым нужно вызывать tick при заданной макс. скорости 450 | uint32_t getMinPeriod() { 451 | float curSpeed; 452 | if (_curMode == KEEP_SPEED) { 453 | curSpeed = abs(_speed); 454 | if (abs(_accelSpeed) > curSpeed) curSpeed = abs(_accelSpeed); 455 | } else curSpeed = _maxSpeed; 456 | return (1000000.0 / curSpeed); 457 | } 458 | 459 | // время между шагами 460 | uint32_t stepTime = 10000; 461 | 462 | using Stepper<_DRV, _TYPE>::pos; 463 | using Stepper<_DRV, _TYPE>::dir; 464 | using Stepper<_DRV, _TYPE>::step; 465 | using Stepper<_DRV, _TYPE>::enable; 466 | using Stepper<_DRV, _TYPE>::disable; 467 | 468 | // ========================= PRIVATE ========================== 469 | private: 470 | // сброс перемещения 471 | void resetMotor() { 472 | _accelSpeed = 0; 473 | #ifdef SMOOTH_ALGORITHM 474 | _n = 0; 475 | #endif 476 | } 477 | // аккуратно сбросить все таймеры 478 | void resetTimers() { 479 | _speedPlannerTime = _plannerTime = _prevTime = micros(); 480 | } 481 | 482 | #ifdef SMOOTH_ALGORITHM 483 | // ========================= PLANNER 1 ========================= 484 | // планировщик скорости из AccelStepper 485 | bool plannerSmooth() { 486 | int32_t err = _target - pos; 487 | int32_t stepsToStop = (float)_accelSpeed * _accelSpeed * _accelInv; 488 | 489 | if (err == 0 && stepsToStop <= 1) return false; 490 | 491 | if (err > 0) { 492 | if (_n > 0) { 493 | if ((stepsToStop >= err) || dir == -1) 494 | _n = -stepsToStop; 495 | } else if (_n < 0) { 496 | if ((stepsToStop < err) && dir == 1) 497 | _n = -_n; 498 | } 499 | } else if (err < 0) { 500 | if (_n > 0) { 501 | if ((stepsToStop >= -err) || dir == 1) 502 | _n = -stepsToStop; 503 | } else if (_n < 0) { 504 | if ((stepsToStop < -err) && dir == -1) 505 | _n = -_n; 506 | } 507 | } 508 | 509 | if (_n == 0) { 510 | _cn = _c0; 511 | dir = _sign(err); 512 | } else { 513 | _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); 514 | _cn = max(_cn, _cmin); 515 | } 516 | _n++; 517 | stepTime = _cn; 518 | _accelSpeed = 1000000.0 / _cn; 519 | if (dir == -1) _accelSpeed = -_accelSpeed; 520 | return true; 521 | } 522 | 523 | int32_t _n = 0; 524 | float _c0 = 0.0; 525 | float _cn = 0.0; 526 | float _cmin = 1.0; 527 | #else 528 | // ========================= PLANNER 2 ========================= 529 | // планировщик скорости мой 530 | void planner() { 531 | if (tickUs - _plannerTime >= _plannerPrd) { 532 | _plannerTime += _plannerPrd; 533 | // ~110 us 534 | int32_t err = _target - pos; // "ошибка" 535 | bool thisDir = (_accelSpeed * _accelSpeed * _accelInv >= abs(err)); // пора тормозить 536 | _accelSpeed += (_accelTime * _plannerPrd * (thisDir ? -_sign(_accelSpeed) : _sign(err))); // разгон/торможение 537 | if (_stopSpeed == 0) _accelSpeed = constrain(_accelSpeed, -_maxSpeed, _maxSpeed); // ограничение 538 | else _accelSpeed = constrain(_accelSpeed, -_stopSpeed, _stopSpeed); 539 | 540 | if (abs(_accelSpeed) > _MIN_SPEED_FP) stepTime = abs(1000000.0 / _accelSpeed); // ограничение на мин. скорость 541 | else stepTime = _MAX_PERIOD_FP; 542 | dir = _sign(_accelSpeed); // направление для шагов 543 | } 544 | } 545 | 546 | uint16_t _plannerPrd = 15000; 547 | #endif 548 | 549 | // ======================= SPEED PLANNER ======================= 550 | float _accelTime = 0; 551 | uint16_t _speedPlannerPrd = 15000; 552 | uint32_t _speedPlannerTime = 0; 553 | uint32_t _plannerTime = 0; 554 | 555 | // планировщик разгона для KEEP_SPEED 556 | void smoothSpeedPlanner() { 557 | if (tickUs - _speedPlannerTime >= _speedPlannerPrd) { 558 | _speedPlannerTime = tickUs; 559 | _accelSpeed += (_accelTime * _speedPlannerPrd * _sign(_speed - _accelSpeed)); 560 | dir = _sign(_accelSpeed); 561 | stepTime = abs(1000000.0 / _accelSpeed); 562 | if (_stopF && abs(_accelSpeed) <= _MIN_STEP_SPEED) brake(); 563 | } 564 | } 565 | 566 | // ========================= VARIABLES ========================= 567 | bool _stopF = 0; 568 | float _stepsPerDeg; 569 | uint32_t _prevTime = 0; 570 | float _accelSpeed = 0; 571 | int32_t _target = 0; 572 | volatile uint32_t tickUs = 0; 573 | bool _workState = false; 574 | bool _autoPower = false; 575 | float _stopSpeed = 0; 576 | float _maxSpeed = 300; 577 | float _speed = 0; 578 | uint16_t _accel = 0; 579 | float _accelInv = 0; 580 | GS_runMode _curMode = FOLLOW_POS; 581 | }; 582 | #endif -------------------------------------------------------------------------------- /src/GyverStepper2.h: -------------------------------------------------------------------------------- 1 | /* 2 | Облегчённая GyverStepper 3 | - Легче на несколько кБ, всё целочисленное 4 | - Более эффективный гибридный алгоритм 5 | - Движение к цели с ускорением 6 | - Макс. скорость: 7 | - Обычный режим: 37000 шаг/с на полной, 18000 шаг/с на разгоне 8 | - Быстрый профиль: 37000 шаг/с на полной, 37000 шаг/с на разгоне 9 | - Движение от точки к точке. Смена точки во время движения не будет плавной 10 | - Вращение со скоростью (без плавной смены скорости) 11 | - Оптимизировано для работы по прерыванию таймера 12 | - Наследует класс Stepper из StepperCore 13 | 14 | AlexGyver, alex@alexgyver.ru 15 | https://alexgyver.ru/ 16 | MIT License 17 | */ 18 | 19 | /* 20 | // ======== ИНИЦИАЛИЗАЦИЯ как в GStepper ======== 21 | GStepper2 stepper(шаговНаОборот, step, dir); // драйвер step-dir 22 | GStepper2 stepper(шаговНаОборот, step, dir, en); // драйвер step-dir + пин enable 23 | GStepper2 stepper(шаговНаОборот, pin1, pin2, pin3, pin4); // драйвер 4 пин 24 | GStepper2 stepper(шаговНаОборот, pin1, pin2, pin3, pin4, en); // драйвер 4 пин + enable 25 | GStepper2 stepper(шаговНаОборот, pin1, pin2, pin3, pin4); // драйвер 4 пин полушаг 26 | GStepper2 stepper(шаговНаОборот, pin1, pin2, pin3, pin4, en); // драйвер 4 пин полушаг + enable 27 | 28 | GStepper2 stepper; // виртуальный драйвер step-dir 29 | GStepper2 stepper; // виртуальный драйвер 4 пин 30 | 31 | // ============ КЛАСС ============ 32 | // === наследуется из Stepper ==== 33 | void step(); // сделать шаг 34 | void invertEn(bool val); // инвертировать поведение EN пина 35 | void reverse(bool val); // инвертировать направление мотора 36 | void disable(); // отключить питание и EN 37 | void enable(); // включить питание и EN 38 | void attachStep(void (*handler)(uint8_t)); // подключить обработчик шага 39 | void attachPower(void (*handler)(bool)); // подключить обработчик питания 40 | 41 | int32_t pos; // текущая позиция в шагах 42 | int8_t dir; // направление (1, -1) 43 | 44 | // ========= GStepper2 ========== 45 | // тикер 46 | bool tick(); // тикер движения, вызывать часто. Вернёт true, если мотор движется 47 | bool tickManual(); // ручной тикер для вызова в прерывании таймера с периодом getPeriod(). Вернёт true, если мотор движется 48 | bool ready(); // однократно вернёт true, если мотор доехал до установленной позиции и остановился 49 | 50 | // вращение 51 | void setSpeed(int16_t speed); // установить скорость в шагах/сек и запустить вращение 52 | void setSpeed(float speed); // установить скорость в шагах/сек (float) и запустить вращение 53 | void setSpeedDeg(int16_t speed); // установить скорость в градусах/сек и запустить вращение 54 | void setSpeedDeg(float speed); // установить скорость в градусах/сек (float) и запустить вращение 55 | 56 | // движение к цели 57 | void setTarget(int32_t ntar, GS_posType type = ABSOLUTE); // установить цель в шагах и опционально режим ABSOLUTE/RELATIVE 58 | void setTargetDeg(int32_t ntar, GS_posType type = ABSOLUTE); // установить цель в градусах и опционально режим ABSOLUTE/RELATIVE 59 | int32_t getTarget(); // получить целевую позицию в шагах 60 | 61 | void setAcceleration(uint16_t nA); // установка ускорения в шаг/сек^2 62 | void setMaxSpeed(int speed); // установить скорость движения при следовании к позиции setTarget() в шагах/сек 63 | void setMaxSpeed(float speed); // установить скорость движения при следовании к позиции setTarget() в шагах/сек, float 64 | void setMaxSpeedDeg(int speed); // установить скорость движения при следовании к позиции в град/сек 65 | void setMaxSpeedDeg(float speed); // установить скорость движения при следовании к позиции в град/сек, float 66 | 67 | void setCurrent(int32_t npos); // установить текущую позицию 68 | int32_t getCurrent(); // получить текущую позицию 69 | void reset(); // сбросить текущую позицию в 0 70 | 71 | // всякое 72 | void autoPower(bool mode); // автоотключение мотора при достижении позиции - true (по умолч. false) 73 | uint32_t getPeriod(); // получить текущий период тиков 74 | void brake(); // резко остановить мотор 75 | void pause(); // пауза - доехать до заданной точки и ждать (ready() не вернёт true, пока ты на паузе) 76 | void resume(); // продолжить движение после остановки/паузы 77 | uint8_t getStatus(); // текущий статус: 0 - стоим, 1 - едем, 2 - едем к точке паузы, 3 - крутимся со скоростью, 4 - тормозим 78 | 79 | // ===== ДЕФАЙНЫ НАСТРОЕК ===== 80 | // дефайнить перед подключением библиотеки 81 | #define GS_NO_ACCEL // отключить модуль движения с ускорением (уменьшить вес кода) 82 | */ 83 | 84 | #ifndef _GyverStepper2_h 85 | #define _GyverStepper2_h 86 | #include 87 | 88 | #include "StepperCore.h" 89 | #define GS_MIN_US 300000 // период, длиннее которого мотор можно резко тормозить или менять скорость 90 | 91 | template 92 | class GStepper2 : public Stepper<_DRV, _TYPE> { 93 | public: 94 | // ========================= КОНСТРУКТОР ========================== 95 | GStepper2(uint16_t steps, uint8_t pin1 = 255, uint8_t pin2 = 255, uint8_t pin3 = 255, uint8_t pin4 = 255, uint8_t pin5 = 255) : Stepper<_DRV, _TYPE>(pin1, pin2, pin3, pin4, pin5) { 96 | stepsRev = steps; 97 | setMaxSpeed(100); 98 | setAcceleration(200); 99 | } 100 | 101 | // ============================= TICK ============================= 102 | // тикер. Вернёт true, если мотор движется 103 | bool tick() { 104 | if (status) { 105 | uint32_t thisUs = micros(); 106 | if (thisUs - tmr >= us) { 107 | tmr = thisUs; 108 | tickManual(); 109 | } 110 | } 111 | return status; 112 | } 113 | 114 | // ручной тикер для вызова в прерывании таймера. Вернёт true, если мотор движется 115 | bool tickManual() { 116 | if (!status) return 0; // стоим-выходим 117 | step(); // шаг 118 | 119 | #ifndef GS_NO_ACCEL // движение с ускорением 120 | switch (status) { 121 | case 1: // едем 122 | case 2: // пауза 123 | // https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/ 124 | steps++; 125 | if (steps < s1) { // разгон 126 | #ifndef GS_FAST_PROFILE 127 | us10 -= 2ul * us10 / (4ul * (steps + so1) + 1); 128 | us = (uint32_t)us10 >> 10; 129 | us = constrain(us, usMin, us0); 130 | #else 131 | if ((steps + so1) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 132 | else { 133 | int j = 0; 134 | while ((steps + so1) >= prfS[j]) j++; 135 | us = prfP[j]; 136 | } 137 | #endif 138 | } else if (steps < s2) us = usMin; // постоянная 139 | else if (steps < S) { // торможение 140 | #ifndef GS_FAST_PROFILE 141 | us10 += 2ul * us10 / (4ul * (S - steps) + 1); 142 | us = (uint32_t)us10 >> 10; 143 | us = constrain(us, usMin, us0); 144 | #else 145 | if ((S - steps) >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 146 | else { 147 | int j = 0; 148 | while ((S - steps) >= prfS[j]) j++; 149 | us = prfP[j]; 150 | } 151 | #endif 152 | } else { // приехали 153 | if (revF) { 154 | brake(); 155 | setTarget(bufT); 156 | return status; 157 | } 158 | if (status == 1) readyF = 1; 159 | brake(); 160 | } 161 | return status; 162 | case 4: // плавная остановка 163 | stopStep--; 164 | #ifndef GS_FAST_PROFILE 165 | us10 += 2ul * us10 / (4ul * stopStep + 1); 166 | us = (uint32_t)us10 >> 10; 167 | us = constrain(us, usMin, us0); 168 | #else 169 | if (stopStep >= prfS[GS_FAST_PROFILE - 1]) us = usMin; 170 | else { 171 | int j = 0; 172 | while (stopStep >= prfS[j]) j++; 173 | us = prfP[j]; 174 | } 175 | #endif 176 | if (pos == tar || stopStep <= 0 || us >= us0) brake(); 177 | return status; 178 | } 179 | #else 180 | if (status <= 2 && pos == tar) { 181 | if (status == 1) readyF = 1; 182 | brake(); 183 | } 184 | #endif 185 | return status; 186 | } 187 | 188 | // ============================= SPEED MODE ============================= 189 | // установить скорость вращения 190 | bool setSpeed(int32_t speed) { 191 | if (speed == 0) { 192 | brake(); 193 | return 0; 194 | } 195 | dir = (speed > 0) ? 1 : -1; 196 | us = 1000000L / abs(speed); 197 | status = 3; 198 | if (autoP) enable(); 199 | return 1; 200 | } 201 | 202 | #ifdef __AVR__ 203 | void setSpeed(int speed) { 204 | setSpeed((int32_t)speed); 205 | } 206 | #endif 207 | 208 | // установить скорость вращения float 209 | void setSpeed(double speed) { 210 | if (setSpeed((int32_t)speed)) us = 1000000.0 / abs(speed); 211 | } 212 | 213 | void setSpeedDeg(int speed) { 214 | setSpeed((int32_t)speed * stepsRev / 360L); 215 | } 216 | 217 | void setSpeedDeg(double speed) { 218 | setSpeed((float)speed * stepsRev / 360L); 219 | } 220 | 221 | // =========================== POSITION MODE =========================== 222 | // установить цель и опционально режим 223 | void setTarget(int32_t ntar, GS_posType type = ABSOLUTE) { 224 | if (sp0) { // нулевая скорость 225 | brake(); 226 | readyF = 1; 227 | return; 228 | } 229 | 230 | if (changeSett) { // применяем настройки 231 | usMin = usMinN; 232 | #ifndef GS_NO_ACCEL 233 | V = 1000000L / usMin; 234 | setAcceleration(na); 235 | #endif 236 | changeSett = 0; 237 | } 238 | 239 | if (type == RELATIVE) tar = ntar + pos; 240 | else tar = ntar; 241 | 242 | if (tar == pos) { 243 | brake(); 244 | readyF = 1; 245 | return; 246 | } 247 | 248 | #ifndef GS_NO_ACCEL 249 | revF = 0; 250 | S = abs(tar - pos); 251 | int8_t ndir = (pos < tar) ? 1 : -1; 252 | int32_t v1 = 0; 253 | if (status > 0) v1 = 1000000L / us; 254 | int32_t ss = 0; 255 | if (a > 0) ss = (int32_t)v1 * v1 / (2L * a); // расстояние до остановки с текущей скоростью 256 | if (ss > S || (status && ndir != dir)) { // не успеем остановиться или едем не туда 257 | revF = 1; 258 | bufT = tar; 259 | tar = pos + ss * dir; 260 | S = ss; 261 | } 262 | 263 | // расчёт точек смены характера движения 264 | // s1 - окончание разгона, s1-s2 - равномерное движение, s2 - торможение 265 | if (a > 0 && usMin < GS_MIN_US) { // ускорение задано и мин. скорость выше порога 266 | if ((int32_t)V * V / a - ((int32_t)v1 * v1 / a >> 1) > S) { // треугольник 267 | if (revF) s1 = 0; 268 | else s1 = ((int32_t)S >> 1) - ((int32_t)v1 * v1 / a >> 2); 269 | s2 = s1; 270 | } else { // трапеция 271 | s1 = ((int32_t)V * V - (int32_t)v1 * v1) / (2L * a); 272 | s2 = S - (int32_t)V * V / (2L * a); 273 | } 274 | so1 = (int32_t)v1 * v1 / (2L * a); 275 | if (v1 == 0) us = us0; 276 | } else { 277 | s1 = so1 = 0; 278 | s2 = S; 279 | us = usMin; 280 | } 281 | // здесь us10 - us*1024 для повышения разрешения микросекунд в 1024 раз 282 | us10 = (uint32_t)us << 10; 283 | steps = 0; 284 | #else 285 | us = usMin; 286 | #endif 287 | dir = (pos < tar) ? 1 : -1; 288 | status = 1; 289 | if (autoP) enable(); 290 | readyF = 0; 291 | } 292 | 293 | // установить цель в градусах и опционально режим 294 | void setTargetDeg(int32_t ntar, GS_posType type = ABSOLUTE) { 295 | setTarget((int32_t)ntar * stepsRev / 360L, type); 296 | } 297 | 298 | // установить цель в градусах float и опционально режим 299 | void setTargetDeg(double ntar, GS_posType type = ABSOLUTE) { 300 | setTarget(ntar * stepsRev / 360.0, type); 301 | } 302 | 303 | // получить целевую позицию 304 | int32_t getTarget() { 305 | #ifndef GS_NO_ACCEL 306 | return revF ? bufT : tar; 307 | #else 308 | return tar; 309 | #endif 310 | } 311 | 312 | // установить текущую позицию 313 | void setCurrent(int32_t npos) { 314 | pos = npos; 315 | } 316 | 317 | // получить текущую позицию 318 | int32_t getCurrent() { 319 | return pos; 320 | } 321 | 322 | // сбросить текущую позицию в 0 323 | void reset() { 324 | pos = 0; 325 | } 326 | 327 | // ========================== POSITION SETTINGS ========================== 328 | // установка ускорения в шаг/сек^2 329 | void setAcceleration(uint16_t acc) { 330 | #ifndef GS_NO_ACCEL 331 | na = acc; 332 | if (!status) { // применяем, если мотор остановлен 333 | a = na; 334 | if (a > 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); 335 | else us0 = usMin; 336 | changeSett = 0; 337 | calcPlan(); 338 | } else changeSett = 1; // иначе флаг на изменение 339 | #endif 340 | } 341 | 342 | // установить скорость движения при следовании к позиции в шагах/сек 343 | void setMaxSpeed(double speed) { 344 | if (speed == 0) { 345 | sp0 = 1; 346 | return; 347 | } 348 | sp0 = 0; 349 | usMinN = 1000000.0 / speed; 350 | if (!status) { // применяем, если мотор остановлен 351 | usMin = usMinN; 352 | #ifndef GS_NO_ACCEL 353 | V = (uint16_t)speed; // если < 1, отсечётся до 0 354 | setAcceleration(a); 355 | #endif 356 | changeSett = 0; 357 | } else changeSett = 1; // иначе флаг на изменение 358 | } 359 | 360 | // установить скорость движения при следовании к позиции в град/сек, float 361 | void setMaxSpeedDeg(double speed) { 362 | setMaxSpeed(speed * stepsRev / 360.0); 363 | } 364 | 365 | // =========================== PLANNER ============================ 366 | // остановить плавно (с заданным ускорением) 367 | void stop() { 368 | #ifndef GS_NO_ACCEL 369 | if (a == 0 || us > GS_MIN_US || status == 3 || !status) { // нет ускорения или медленно едем - дёргай ручник 370 | brake(); 371 | return; 372 | } 373 | if (status <= 2) { // едем 374 | if (steps > s2) { // а мы уже тормозим! 375 | pause(); // значит флаг на паузу 376 | return; 377 | } 378 | stopStep = 1000000ul / us; // наша скорость 379 | stopStep = (uint32_t)stopStep * stopStep / 2 / a; // дистанция остановки. a не может быть 0 380 | us10 = (uint32_t)us << 10; 381 | status = 4; 382 | } 383 | #else 384 | brake(); 385 | #endif 386 | } 387 | 388 | // автоотключение мотора при достижении позиции - true (по умолч. false) 389 | void autoPower(bool mode) { 390 | autoP = mode; 391 | } 392 | 393 | // остановить мотор 394 | void brake() { 395 | status = 0; 396 | if (autoP) disable(); 397 | } 398 | 399 | // пауза (доехать до заданной точки и ждать). ready() не вернёт true, пока ты на паузе 400 | void pause() { 401 | if (status == 1) status = 2; 402 | } 403 | 404 | // продолжить движение после остановки 405 | void resume() { 406 | if (!status) setTarget(tar); 407 | } 408 | 409 | // текущий статус: 0 - стоим, 1 - едем, 2 - едем к точке паузы, 3 - крутимся со скоростью, 4 - тормозим 410 | uint8_t getStatus() { 411 | return status; 412 | } 413 | 414 | // вернёт true, если мотор доехал до установленной позиции и остановился 415 | bool ready() { 416 | if (!status && readyF) { 417 | readyF = 0; 418 | return 1; 419 | } 420 | return 0; 421 | } 422 | 423 | // получить текущий период вращения 424 | uint32_t getPeriod() { 425 | return us; 426 | } 427 | 428 | // чики пуки 429 | using Stepper<_DRV, _TYPE>::pos; 430 | using Stepper<_DRV, _TYPE>::dir; 431 | using Stepper<_DRV, _TYPE>::step; 432 | using Stepper<_DRV, _TYPE>::enable; 433 | using Stepper<_DRV, _TYPE>::disable; 434 | 435 | // ============================= PRIVATE ============================= 436 | private: 437 | void calcPlan() { 438 | #ifdef GS_FAST_PROFILE 439 | if (a > 0) { 440 | uint32_t sa = (uint32_t)V * V / a / 2ul; // расстояние разгона 441 | float dtf = sqrt(2.0 * sa / a) / GS_FAST_PROFILE; // время участка профиля 442 | float s0 = a * dtf * dtf / 2.0; // первый участок профиля 443 | uint32_t dt = dtf * 1000000.0; // время участка в секундах 444 | for (int i = 0; i < GS_FAST_PROFILE; i++) { 445 | prfS[i] = s0 * (i + 1) * (i + 1); 446 | uint32_t ds = prfS[i]; 447 | if (i > 0) ds -= prfS[i - 1]; 448 | if (ds <= 0) prfP[i] = 0; 449 | else prfP[i] = (uint32_t)dt / ds; 450 | } 451 | } 452 | #endif 453 | } 454 | 455 | uint32_t tmr = 0, us = 10000, usMin = 10000; 456 | int32_t tar = 0; 457 | uint16_t stepsRev; 458 | uint8_t status = 0; 459 | bool readyF = 0; 460 | bool changeSett = 0; 461 | bool autoP = false; 462 | uint32_t usMinN; 463 | bool sp0 = 0; 464 | 465 | #ifndef GS_NO_ACCEL 466 | uint16_t a, V; 467 | uint16_t na; 468 | int16_t stopStep; 469 | uint32_t us0, us10; 470 | int32_t S, s1, s2, so1, steps; 471 | int32_t bufT = 0; 472 | bool revF = false; 473 | 474 | #ifdef GS_FAST_PROFILE 475 | uint32_t prfS[GS_FAST_PROFILE], prfP[GS_FAST_PROFILE]; 476 | #endif 477 | #endif 478 | }; 479 | #endif -------------------------------------------------------------------------------- /src/StepperCore.h: -------------------------------------------------------------------------------- 1 | /* 2 | Ядро библиотеки для управления шаговыми моторами: 3 | - 4 фазные и STEP DIR драйверы 4 | - Поддержка пина EN 5 | - Виртуальный драйвер 6 | - Быстрый алгоритм IO для AVR 7 | 8 | AlexGyver, alex@alexgyver.ru 9 | https://alexgyver.ru/ 10 | MIT License 11 | */ 12 | 13 | /* 14 | // ======== ИНИЦИАЛИЗАЦИЯ ======== 15 | Stepper stepper(step, dir); // драйвер step-dir 16 | Stepper stepper(step, dir, en); // драйвер step-dir + пин enable 17 | Stepper stepper(pin1, pin2, pin3, pin4); // драйвер 4 пин 18 | Stepper stepper(pin1, pin2, pin3, pin4, en); // драйвер 4 пин + enable 19 | Stepper stepper(pin1, pin2, pin3, pin4); // драйвер 4 пин полушаг 20 | Stepper stepper(pin1, pin2, pin3, pin4, en); // драйвер 4 пин полушаг + enable 21 | 22 | Stepper stepper; // виртуальный драйвер step-dir 23 | Stepper stepper; // виртуальный драйвер 4 пин 24 | 25 | // ============ КЛАСС ============ 26 | void step(); // сделать шаг 27 | void invertEn(bool val); // инвертировать поведение EN пина 28 | void reverse(bool val); // инвертировать направление мотора 29 | void disable(); // отключить питание и EN 30 | void enable(); // включить питание и EN 31 | void power(bool); // переключить питание 32 | void attachStep(void (*handler)(uint8_t)); // подключить обработчик шага 33 | void attachPower(void (*handler)(bool)); // подключить обработчик питания 34 | 35 | int32_t pos; // текущая позиция в шагах 36 | int8_t dir; // направление (1, -1) 37 | */ 38 | 39 | #ifndef _StepperCore_h 40 | #define _StepperCore_h 41 | #include 42 | 43 | #include "GStypes.h" 44 | 45 | #ifndef DRIVER_STEP_TIME 46 | #define DRIVER_STEP_TIME 4 47 | #endif 48 | 49 | #define _PINS_AMOUNT ((_TYPE == STEPPER_PINS) ? (_DRV == 0 ? 2 : 4) : (0)) 50 | 51 | template 52 | class Stepper { 53 | public: 54 | Stepper(uint8_t pin1 = 255, uint8_t pin2 = 255, uint8_t pin3 = 255, uint8_t pin4 = 255, uint8_t pin5 = 255) { 55 | if (_TYPE == STEPPER_PINS) { 56 | if (_DRV == STEPPER2WIRE) { 57 | configurePin(0, pin1); 58 | configurePin(1, pin2); 59 | if (pin3 != 255) { 60 | _enPin = pin3; 61 | pinMode(_enPin, OUTPUT); 62 | } 63 | } else { 64 | configurePin(0, pin1); 65 | configurePin(1, pin2); 66 | configurePin(2, pin3); 67 | configurePin(3, pin4); 68 | if (pin5 != 255) { 69 | _enPin = pin5; 70 | pinMode(_enPin, OUTPUT); 71 | } 72 | } 73 | } 74 | } 75 | 76 | // сделать шаг 77 | void step() { 78 | pos += dir; 79 | if (_DRV == STEPPER2WIRE) { // ~4 + DRIVER_STEP_TIME us 80 | stepDir(); 81 | } else { // ~5.5 us 82 | thisStep += (_globDir ? dir : -dir); 83 | step4(); 84 | } 85 | } 86 | 87 | // инвертировать поведение EN пина 88 | void invertEn(bool val) { 89 | _enDir = val; 90 | } 91 | 92 | // инвертировать направление мотора 93 | void reverse(bool val) { 94 | if (_DRV == STEPPER2WIRE) { 95 | if (_TYPE == STEPPER_PINS && _globDir != val) setPin(1, (dir > 0) ^ val); 96 | } 97 | _globDir = val; 98 | } 99 | 100 | // отключить питание и EN 101 | void disable() { 102 | if (_TYPE == STEPPER_PINS) { 103 | if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) { 104 | setPin(0, 0); 105 | setPin(1, 0); 106 | setPin(2, 0); 107 | setPin(3, 0); 108 | } 109 | if (_enPin != 255) digitalWrite(_enPin, !_enDir); 110 | } else { 111 | if (*_power) _power(0); 112 | if (*_step && (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF)) _step(0); 113 | } 114 | } 115 | 116 | // включить питание и EN 117 | void enable() { 118 | if (_TYPE == STEPPER_PINS) { 119 | // подадим прошлый сигнал на мотор, чтобы вал зафиксировался 120 | if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) step4(); 121 | if (_enPin != 255) digitalWrite(_enPin, _enDir); 122 | } else { 123 | if (*_power) _power(1); 124 | if (*_step && (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF)) step4(); 125 | } 126 | } 127 | 128 | // переключить питание 129 | void power(bool state) { 130 | if (state) enable(); 131 | else disable(); 132 | } 133 | 134 | // подключить обработчик шага 135 | void attachStep(void (*handler)(uint8_t)) { 136 | _step = handler; 137 | } 138 | 139 | // подключить обработчик питания 140 | void attachPower(void (*handler)(bool)) { 141 | _power = handler; 142 | } 143 | 144 | int32_t pos = 0; 145 | int8_t dir = 1; 146 | 147 | private: 148 | // настройка пина 149 | void configurePin(int num, uint8_t pin) { 150 | pinMode(pin, OUTPUT); 151 | #ifdef __AVR__ 152 | _port_reg[num] = portOutputRegister(digitalPinToPort(pin)); 153 | _bit_mask[num] = digitalPinToBitMask(pin); 154 | #else 155 | _pins[num] = pin; 156 | #endif 157 | } 158 | 159 | // быстрая установка пина 160 | void setPin(int num, bool state) { 161 | #ifdef __AVR__ 162 | if (state) *_port_reg[num] |= _bit_mask[num]; 163 | else *_port_reg[num] &= ~_bit_mask[num]; 164 | #elif defined(ESP8266) 165 | if (state) GPOS = (1 << _pins[num]); 166 | else GPOC = (1 << _pins[num]); 167 | #else 168 | digitalWrite(_pins[num], state); 169 | #endif 170 | } 171 | 172 | // шаг для 4 фаз 173 | void step4() { 174 | if (_TYPE == STEPPER_PINS) { 175 | if (_DRV == STEPPER4WIRE) { 176 | // 0b11 берёт два бита, т.е. формирует 0 1 2 3 0 1.. 177 | switch (thisStep & 0b11) { 178 | case 0: 179 | setPin(0, 1); 180 | setPin(1, 0); 181 | setPin(2, 1); 182 | setPin(3, 0); 183 | break; // 1010 184 | case 1: 185 | setPin(0, 0); 186 | setPin(1, 1); 187 | setPin(2, 1); 188 | setPin(3, 0); 189 | break; // 0110 190 | case 2: 191 | setPin(0, 0); 192 | setPin(1, 1); 193 | setPin(2, 0); 194 | setPin(3, 1); 195 | break; // 0101 196 | case 3: 197 | setPin(0, 1); 198 | setPin(1, 0); 199 | setPin(2, 0); 200 | setPin(3, 1); 201 | break; // 1001 202 | } 203 | } else if (_DRV == STEPPER4WIRE_HALF) { 204 | // 0b111 берёт три бита, т.е. формирует 0 1 2 4 5 6 7 0 1 2.. 205 | switch (thisStep & 0b111) { 206 | case 0: 207 | setPin(0, 1); 208 | setPin(1, 0); 209 | setPin(2, 0); 210 | setPin(3, 0); 211 | break; // 1000 212 | case 1: 213 | setPin(0, 1); 214 | setPin(1, 0); 215 | setPin(2, 1); 216 | setPin(3, 0); 217 | break; // 1010 218 | case 2: 219 | setPin(0, 0); 220 | setPin(1, 0); 221 | setPin(2, 1); 222 | setPin(3, 0); 223 | break; // 0010 224 | case 3: 225 | setPin(0, 0); 226 | setPin(1, 1); 227 | setPin(2, 1); 228 | setPin(3, 0); 229 | break; // 0110 230 | case 4: 231 | setPin(0, 0); 232 | setPin(1, 1); 233 | setPin(2, 0); 234 | setPin(3, 0); 235 | break; // 0100 236 | case 5: 237 | setPin(0, 0); 238 | setPin(1, 1); 239 | setPin(2, 0); 240 | setPin(3, 1); 241 | break; // 0101 242 | case 6: 243 | setPin(0, 0); 244 | setPin(1, 0); 245 | setPin(2, 0); 246 | setPin(3, 1); 247 | break; // 0001 248 | case 7: 249 | setPin(0, 1); 250 | setPin(1, 0); 251 | setPin(2, 0); 252 | setPin(3, 1); 253 | break; // 1001 254 | } 255 | } 256 | } else if (*_step) { 257 | if (_DRV == STEPPER4WIRE) { 258 | switch (thisStep & 0b11) { 259 | case 0: 260 | _step(0b1010); 261 | break; // 1010 262 | case 1: 263 | _step(0b0110); 264 | break; // 0110 265 | case 2: 266 | _step(0b0101); 267 | break; // 0101 268 | case 3: 269 | _step(0b1001); 270 | break; // 1001 271 | } 272 | } else if (_DRV == STEPPER4WIRE_HALF) { 273 | switch (thisStep & 0b111) { 274 | case 0: 275 | _step(0b1000); 276 | break; // 1000 277 | case 1: 278 | _step(0b1010); 279 | break; // 1010 280 | case 2: 281 | _step(0b0010); 282 | break; // 0010 283 | case 3: 284 | _step(0b0110); 285 | break; // 0110 286 | case 4: 287 | _step(0b0100); 288 | break; // 0100 289 | case 5: 290 | _step(0b0101); 291 | break; // 0101 292 | case 6: 293 | _step(0b0001); 294 | break; // 0001 295 | case 7: 296 | _step(0b1001); 297 | break; // 1001 298 | } 299 | } 300 | } 301 | } 302 | 303 | // шажочек степдир 304 | void stepDir() { 305 | if (_TYPE == STEPPER_PINS) { 306 | if (_pdir != dir) { 307 | _pdir = dir; 308 | setPin(1, (dir > 0) ^ _globDir); // DIR 309 | } 310 | setPin(0, 1); // step HIGH 311 | if (DRIVER_STEP_TIME > 0) delayMicroseconds(DRIVER_STEP_TIME); 312 | setPin(0, 0); // step LOW 313 | } else if (*_step) { 314 | _step((dir > 0) ^ _globDir); 315 | } 316 | } 317 | 318 | int8_t _enPin = 255; 319 | bool _enDir = false; 320 | bool _globDir = false; 321 | int8_t _pdir = 0; 322 | int8_t thisStep = 0; 323 | 324 | void (*_step)(uint8_t a) = NULL; 325 | void (*_power)(bool a) = NULL; 326 | 327 | #ifdef __AVR__ 328 | volatile uint8_t *_port_reg[_PINS_AMOUNT]; 329 | volatile uint8_t _bit_mask[_PINS_AMOUNT]; 330 | #else 331 | uint8_t _pins[_PINS_AMOUNT]; 332 | #endif 333 | }; 334 | 335 | #endif 336 | --------------------------------------------------------------------------------