Skip to content

Commit

Permalink
Обновить pxt.json, main.ts, lw.ts, turns.ts
Browse files Browse the repository at this point in the history
  • Loading branch information
@THEb0nny
THEb0nny committed Mar 27, 2024
1 parent 4662531 commit 16282f1
Show file tree
Hide file tree
Showing 4 changed files with 152 additions and 28 deletions.
174 changes: 148 additions & 26 deletions lw.ts
View file
Original file line number Diff line number Diff line change
@@ -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(); // Сброс ПИД регулятора
Expand All @@ -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);
Expand All @@ -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); // Действие после алгоритма движения
}

/**
Expand All @@ -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; // Дистанция в мм, которую нужно проехать по линии

Expand All @@ -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);
Expand All @@ -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);
}

/**
Expand All @@ -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(); // Сброс регулятора
Expand Down Expand Up @@ -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(); // Сброс регулятора
Expand Down Expand Up @@ -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(); // Сброс регулятора
Expand Down Expand Up @@ -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(); // Сброс регулятора
Expand Down
2 changes: 2 additions & 0 deletions main.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; // Интегральный коэффициент регулятора движения по линии на левом датчике
Expand Down
2 changes: 1 addition & 1 deletion pxt.json
View file
Original file line number Diff line number Diff line change
@@ -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": "*",
Expand Down
2 changes: 1 addition & 1 deletion turns.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down

0 comments on commit 16282f1

Please sign in to comment.