From 16282f19fcafd544d83d74c51b5173d88eca7c1a Mon Sep 17 00:00:00 2001 From: Dmitriy Antipov Date: Wed, 27 Mar 2024 15:33:01 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=B8=D1=82?= =?UTF-8?q?=D1=8C=20pxt.json,=20main.ts,=20lw.ts,=20turns.ts?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lw.ts | 174 ++++++++++++++++++++++++++++++++++++++++++++++--------- main.ts | 2 + pxt.json | 2 +- turns.ts | 2 +- 4 files changed, 152 insertions(+), 28 deletions(-) diff --git a/lw.ts b/lw.ts index d63a61f..bec1b18 100644 --- a/lw.ts +++ b/lw.ts @@ -1,20 +1,133 @@ +namespace motions { + + // Интерфейс перадачи параметров движения по линии + export interface LineFollowInreface { + speed?: number; + Kp?: number; + Ki?: number; + Kd?: number; + N?: number; + } + + /** + * Пустые праметры для движения. + */ + //% blockId="SetLineFollowEmptyParams" + //% block="empty" + //% block.loc.ru="пусто" + //% inlineInputMode="inline" + //% blockHidden=true + //% weight="99" + //% group="Params" + export function SetLineFollowEmptyParams(): LineFollowInreface { + return null; + } + + /** + * Параметры для движения с возможностью установить скорость, Kp. + */ + //% blockId="SetLineFollow1Params" + //% block="speed = $newSpeed\\%" + //% block.loc.ru="скорость = $newSpeed\\%" + //% inlineInputMode="inline" + //% newSpeed.defl="50" + //% newKp.defl="1" + //% weight="98" + //% group="Params" + export function SetLineFollow1Params(newSpeed?: number): LineFollowInreface { + return { + speed: newSpeed + }; + } + + /** + * Параметры для движения с возможностью установить скорость, Kp. + */ + //% blockId="SetLineFollow2Params" + //% block="speed = $newSpeed\\%| Kp = $newKp" + //% block.loc.ru="скорость = $newSpeed\\%| Kp = $newKp" + //% inlineInputMode="inline" + //% newSpeed.defl="50" + //% newKp.defl="1" + //% weight="97" + //% group="Params" + export function SetLineFollow2Params(newSpeed?: number, newKp?: number): LineFollowInreface { + return { + speed: newSpeed, + Kp: newKp + }; + } + + /** + * Параметры для движения с возможностью установить скорость, Kp, Kd, и N - фильтр дифференциального регулятора. + */ + //% blockId="SetLineFollow4Params" + //% block="speed = $newSpeed\\%| Kp = $newKp| Kd = $newKd|| N = $newN" + //% block.loc.ru="скорость = $newSpeed\\%| Kp = $newKp| Kd = $newKd|| N = $newN" + //% expandableArgumentMode="enabled" + //% inlineInputMode="inline" + //% newSpeed.defl="50" + //% newKp.defl="1" + //% weight="96" + //% group="Params" + export function SetLineFollow4Params(newSpeed?: number, newKp?: number, newKd?: number, newN?: number): LineFollowInreface { + return { + speed: newSpeed, + Kp: newKp, + Kd: newKd, + N: newN + }; + } + + /** + * Параметры для движения с возможностью установить скорость, Kp, Ki, Kd, и N - фильтр дифференциального регулятора. + */ + //% blockId="SetLineFollowAllParams" + //% block="speed = $newSpeed\\%| Kp = $newKp| Ki = $newKi| Kd = $newKd|| N = $newN" + //% block.loc.ru="скорость = $newSpeed\\%| Kp = $newKp| Ki = $newKi| Kd = $newKd|| N = $newN" + //% expandableArgumentMode="enabled" + //% inlineInputMode="inline" + //% newSpeed.defl="50" + //% newKp.defl="1" + //% weight="95" + //% group="Params" + export function SetLineFollowAllParams(newSpeed?: number, newKp?: number, newKi?: number, newKd?: number, newN?: number): LineFollowInreface { + return { + speed: newSpeed, + Kp: newKp, + Ki: newKi, + Kd: newKd, + N: newN + }; + } + +} + namespace motions { /** * Функция движения по линии до перекрёстка. - * @param speed скорость движения, eg: 60 * @param actionAfterMotion действие после перекрёстка, eg: AfterMotion.Rolling * @param debug отладка, eg: false */ - //% blockId="LineFollowToIntersaction" - //% block="движение по линии до перекрёстка на $speed|\\% c действием после $actionAfterMotion||отладка $debug" - //% expandableArgumentMode="toggle" + //% blockId="LineFollowToIntersection" + //% block="движение по линии до перекрёстка с действием после $actionAfterMotion||параметры = $params| отладка $debug" + //% expandableArgumentMode="enabled" //% inlineInputMode="inline" //% speed.shadow="motorSpeedPicker" //% debug.shadow="toggleOnOff" - //% weight="1" + //% params.shadow=SetLineFollowEmptyParams + //% weight="99" //% group="Движение по линии" - export function LineFollowToIntersaction(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + export function LineFollowToIntersection(actionAfterMotion: AfterMotion, params?: LineFollowInreface, debug: boolean = false) { + if (params) { + if (params.speed) LW_SPEED_2S = params.speed; + if (params.Kp) LW_KP_2S = params.Kp; + if (params.Ki) LW_KI_2S = params.Ki; + if (params.Kd) LW_KD_2S = params.Kd; + if (params.N) LW_N_2S = params.N; + } + automation.pid1.setGains(LW_KP_2S, LW_KI_2S, LW_KD_2S); // Установка коэффицентов ПИД регулятора automation.pid1.setControlSaturation(-200, 200); // Установка интервала ПИД регулятора automation.pid1.reset(); // Сброс ПИД регулятора @@ -32,7 +145,7 @@ namespace motions { let error = refLCS - refRCS; // Ошибка регулирования automation.pid1.setPoint(error); // Передать ошибку регулятору let U = automation.pid1.compute(dt, 0); // Управляющее воздейвствие - CHASSIS_MOTORS.steer(U, speed); // Команда моторам + CHASSIS_MOTORS.steer(U, LW_SPEED_2S); // Команда моторам if (debug) { brick.clearScreen(); // Очистка экрана brick.printValue("refLCS", refLCS, 1); @@ -44,7 +157,7 @@ namespace motions { control.PauseUntilTime(currTime, 10); // Ожидание выполнения цикла } music.PlayToneInParallel(262, BeatFraction.Half); // Издаём сигнал завершения - custom.ActionAfterMotion(speed, actionAfterMotion); + custom.ActionAfterMotion(LW_SPEED_2S, actionAfterMotion); // Действие после алгоритма движения } /** @@ -54,15 +167,24 @@ namespace motions { * @param actionAfterMotion действие после перекрёстка, eg: AfterMotion.Rolling * @param debug отладка, eg: false */ - //% blockId="LineFollowToDist" - //% block="движение по линии на расстояние $dist|на %speed|\\% с действием после $actionAfterMotion||отладка $debug" + //% blockId="LineFollowToDistance" + //% block="движение по линии на расстояние $dist|мм с действием после $actionAfterMotion||параметры = $params| отладка $debug" //% expandableArgumentMode="toggle" //% inlineInputMode="inline" //% speed.shadow="motorSpeedPicker" //% debug.shadow="toggleOnOff" - //% weight="2" + //% params.shadow=SetLineFollowEmptyParams + //% weight="98" //% group="Движение по линии" - export function LineFollowToDist(dist: number, speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + export function LineFollowToDistance(dist: number, actionAfterMotion: AfterMotion, params?: LineFollowInreface, debug: boolean = false) { + if (params) { + if (params.speed) LW_SPEED_2S = params.speed; + if (params.Kp) LW_KP_2S = params.Kp; + if (params.Ki) LW_KI_2S = params.Ki; + if (params.Kd) LW_KD_2S = params.Kd; + if (params.N) LW_N_2S = params.N; + } + let lMotEncPrev = CHASSIS_L_MOTOR.angle(), rMotEncPrev = CHASSIS_R_MOTOR.angle(); // Значения с энкодеров моторов до запуска let calcMotRot = (dist / (Math.PI * WHEELS_D)) * 360; // Дистанция в мм, которую нужно проехать по линии @@ -84,7 +206,7 @@ namespace motions { let error = LW_SET_POINT - refRCS; // Ошибка регулирования automation.pid1.setPoint(error); // Передать ошибку регулятору let U = automation.pid1.compute(dt, 0); // Управляющее воздейвствие - CHASSIS_MOTORS.steer(U, speed); // Команда моторам + CHASSIS_MOTORS.steer(U, LW_SPEED_2S); // Команда моторам if (debug) { brick.clearScreen(); // Очистка экрана brick.printValue("refLCS", refLCS, 1); @@ -96,7 +218,7 @@ namespace motions { control.PauseUntilTime(currTime, 10); // Ожидание выполнения цикла } music.PlayToneInParallel(262, BeatFraction.Half); // Издаём сигнал завершения - custom.ActionAfterMotion(speed, actionAfterMotion); + custom.ActionAfterMotion(LW_SPEED_2S, actionAfterMotion); } /** @@ -112,18 +234,18 @@ namespace motions { //% speed.shadow="motorSpeedPicker" //% debug.shadow="toggleOnOff" //% inlineInputMode="inline" - //% weight="1" + //% weight="89" //% group="Движение по линии" export function LineFollowToLeftIntersaction(lineLocation: LineLocation, speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { if (lineLocation == LineLocation.Inside) { - LineFollowToLeftIntersactionInside(speed, actionAfterMotion, debug); + LineFollowToLeftIntersectionInside(speed, actionAfterMotion, debug); } else if (lineLocation == LineLocation.Outside) { - LineFollowToLeftIntersactionOutside(speed, actionAfterMotion, debug); + LineFollowToLeftIntersectionOutside(speed, actionAfterMotion, debug); } } // Функция движения по линии правым датчиком до перекрёстка слева с линией между датчиками - function LineFollowToLeftIntersactionInside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + function LineFollowToLeftIntersectionInside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { automation.pid1.setGains(LW_KP_RS, LW_KI_RS, LW_KD_RS); // Установка коэффицентов регулятора automation.pid1.setControlSaturation(-200, 200); // Установка диапазона регулирования регулятора automation.pid1.reset(); // Сброс регулятора @@ -155,7 +277,7 @@ namespace motions { } // Функция движения по линии правым датчиком до перекрёстка слева с линией извне - function LineFollowToLeftIntersactionOutside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + function LineFollowToLeftIntersectionOutside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { automation.pid1.setGains(LW_KP_RS, LW_KI_RS, LW_KD_RS); // Установка коэффицентов регулятора automation.pid1.setControlSaturation(-200, 200); // Установка диапазона регулирования регулятора automation.pid1.reset(); // Сброс регулятора @@ -195,24 +317,24 @@ namespace motions { * @param actionAfterMotion действие после перекрёстка, eg: AfterMotion.Rolling * @param debug отладка, eg: false */ - //% blockId="LineFollowToRightIntersaction" + //% blockId="LineFollowToRightIntersection" //% block="движение по линии до перекрёстка справа $lineLocation|на $speed|\\% c действием после $actionAfterMotion||отладка $debug" //% expandableArgumentMode="toggle" //% speed.shadow="motorSpeedPicker" //% debug.shadow="toggleOnOff" //% inlineInputMode="inline" - //% weight="1" + //% weight="79" //% group="Движение по линии" - export function LineFollowToRightIntersaction(lineLocation: LineLocation, speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + export function LineFollowToRightIntersection(lineLocation: LineLocation, speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { if (lineLocation == LineLocation.Inside) { - LineFollowToRightIntersactionInside(speed, actionAfterMotion, debug); + LineFollowToRightIntersectionInside(speed, actionAfterMotion, debug); } else if (lineLocation == LineLocation.Outside) { - LineFollowToRightIntersactionOutside(speed, actionAfterMotion, debug); + LineFollowToRightIntersectionOutside(speed, actionAfterMotion, debug); } } // Функция движения по линии левым датчиком до перекрёстка справа - function LineFollowToRightIntersactionInside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + function LineFollowToRightIntersectionInside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { automation.pid1.setGains(LW_KP_LS, LW_KI_LS, LW_KD_LS); // Установка коэффицентов регулятора automation.pid1.setControlSaturation(-200, 200); // Установка диапазона регулирования регулятора automation.pid1.reset(); // Сброс регулятора @@ -246,7 +368,7 @@ namespace motions { } // Функция движения по линии левым датчиком до перекрёстка справа с линией извне - function LineFollowToRightIntersactionOutside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { + function LineFollowToRightIntersectionOutside(speed: number, actionAfterMotion: AfterMotion, debug: boolean = false) { automation.pid1.setGains(LW_KP_LS, LW_KI_LS, LW_KD_LS); // Установка коэффицентов регулятора automation.pid1.setControlSaturation(-200, 200); // Установка диапазона регулирования регулятора automation.pid1.reset(); // Сброс регулятора diff --git a/main.ts b/main.ts index c9c395a..fdd3924 100644 --- a/main.ts +++ b/main.ts @@ -6,9 +6,11 @@ let W_REF_RAW_RCS = 500; // Сырое значение на белом для let LW_TRESHOLD = 35; // Пороговое значение определения перекрёстка let LW_SET_POINT = 50; // Среднее значение серого +let LW_SPEED_2S = 50; // Скорость движения по линии двумя датчиками let LW_KP_2S = 0.5; // Пропорциональный коэффициент регулятора движения по линии двумя датчиками let LW_KI_2S = 0; // Интегральный коэффициент регулятора движения по линии двумя датчиками let LW_KD_2S = 0; // Дифференциальный коэффициент регулятора движения по линии двумя датчиками +let LW_N_2S = 0; // Фильтр дифференциального регулятора движения по линии двумя датчиками let LW_KP_LS = 0.5; // Пропорциональный коэффициент регулятора движения по линии на левом датчике let LW_KI_LS = 0; // Интегральный коэффициент регулятора движения по линии на левом датчике diff --git a/pxt.json b/pxt.json index b0363a4..ca43a28 100644 --- a/pxt.json +++ b/pxt.json @@ -1,6 +1,6 @@ { "name": "Promrobo21", - "version": "0.4.3", + "version": "0.5.0", "description": "Methods (blocks) for working with ev3 to participate in competitions.", "dependencies": { "ev3": "*", diff --git a/turns.ts b/turns.ts index 1303d7f..1db51c8 100644 --- a/turns.ts +++ b/turns.ts @@ -72,7 +72,7 @@ namespace turns { * @param debug отладка на экран, eg: false */ //% blockId="SmartPivotTurn" - //% block="умный поворот на $deg|° с $speed|\\% относительно $wheelPivot||и отладкой %debug" + //% block="умный поворот на $deg|° с $speed|\\% относительно $wheelPivot|колеса||и отладкой %debug" //% expandableArgumentMode="toggle" //% inlineInputMode="inline" //% speed.shadow="motorSpeedPicker"