Skip to content

Commit

Permalink
Обновить pxt.json, main.ts, lw.ts, motions.ts, turns.ts, sensors.ts
Browse files Browse the repository at this point in the history
  • Loading branch information
@THEb0nny
THEb0nny committed Apr 10, 2024
1 parent 95a0027 commit f7df289
Show file tree
Hide file tree
Showing 6 changed files with 57 additions and 48 deletions.
10 changes: 5 additions & 5 deletions lw.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,8 +162,8 @@ namespace motions {
let lMotEncPrev = CHASSIS_L_MOTOR.angle(), rMotEncPrev = CHASSIS_R_MOTOR.angle(); // Значения с энкодеров моторов до запуска
let calcMotRot = (dist / (Math.PI * WHEELS_D)) * 360; // Дистанция в мм, которую нужно проехать по линии

automation.pid1.setGains(lineFollow2SensorKp, lineFollow2SensorKi, lineFollow2SensorKd); // Установка коэффицентов ПИД регулятора
automation.pid1.setDerivativeFilter(lineFollow2SensorN); // Установить фильтр дифференциального регулятора
automation.pid1.setGains(lineFollowLeftSensorKp, lineFollowLeftSensorKi, lineFollowLeftSensorKd); // Установка коэффицентов ПИД регулятора
automation.pid1.setDerivativeFilter(lineFollowLeftSensorN); // Установить фильтр дифференциального регулятора
automation.pid1.setControlSaturation(-200, 200); // Установка интервала ПИД регулятора
automation.pid1.reset(); // Сброс ПИД регулятора

Expand All @@ -183,8 +183,8 @@ namespace motions {
else if (lineLocation == HorizontalLineLocation.Outside) error = LW_SET_POINT - refLCS; // Ошибка регулирования
automation.pid1.setPoint(error); // Передать ошибку регулятору
let U = automation.pid1.compute(dt, 0); // Управляющее воздействие
//CHASSIS_MOTORS.steer(U, lineFollow2SensorSpeed); // Команда моторам
chassis.ChassisControl(U, lineFollow2SensorSpeed);
//CHASSIS_MOTORS.steer(U, lineFollowLeftSensorSpeed); // Команда моторам
chassis.ChassisControl(U, lineFollowLeftSensorSpeed);
if (debug) {
brick.clearScreen(); // Очистка экрана
brick.printValue("refLCS", refLCS, 1);
Expand All @@ -196,7 +196,7 @@ namespace motions {
control.pauseUntilTime(currTime, 10); // Ожидание выполнения цикла
}
music.playToneInBackground(262, 300); // Издаём сигнал завершения
chassis.ActionAfterMotion(lineFollow2SensorSpeed, actionAfterMotion); // Действие после алгоритма движения
chassis.ActionAfterMotion(lineFollowLeftSensorSpeed, actionAfterMotion); // Действие после алгоритма движения
}

/**
Expand Down
76 changes: 43 additions & 33 deletions main.ts
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
//let CHASSIS_MOTORS = motors.largeBC; // Ссылка на объект моторов в шасси
let CHASSIS_L_MOTOR = motors.largeB; // Ссылка на объект левого мотора в шасси
let CHASSIS_R_MOTOR = motors.largeC; // Ссылка на объект правого мотора в шасси
let CHASSIS_L_MOTOR = motors.mediumB; // Ссылка на объект левого мотора в шасси
let CHASSIS_R_MOTOR = motors.mediumC; // Ссылка на объект правого мотора в шасси

let MANIPULATOR_MOTOR1: motors.Motor = motors.mediumA; // Ссылка на объект мотора манипулятора
let MANIPULATOR_MOTOR2: motors.Motor = motors.mediumD; // Ссылка на объект мотора манипулятора
Expand Down Expand Up @@ -35,20 +35,21 @@ function RgbToHsvlToColorConvert(debug: boolean = false) {
prevTime = currTime; // Новое время в переменную предыдущего времени
let rgbCS = CHECK_COLOR_CS.rgbRaw();
for (let i = 0; i < 3; i++) {
rgbCS[i] = Math.map(rgbCS[i], 0, sensors.maxRgbValuesCS4[i], 0, 255);
rgbCS[i] = Math.map(rgbCS[i], 0, sensors.maxRgbColorSensor4[i], 0, 255);
rgbCS[i] = Math.constrain(rgbCS[i], 0, 255);
}
const hsvlCS = sensors.RgbToHsvlConverter(rgbCS);
const color = sensors.HsvToColorNum(hsvlCS);
if (debug) {
brick.clearScreen();
brick.printValue("r", rgbCS[0], 1, 20);
brick.printValue("g", rgbCS[1], 2, 20);
brick.printValue("b", rgbCS[2], 3, 20);
brick.printValue("hue", hsvlCS[0], 5, 20);
brick.printValue("sat", hsvlCS[1], 6, 20);
brick.printValue("val", hsvlCS[2], 7, 20);
brick.printValue("light", hsvlCS[3], 8, 20);
brick.printValue("color", color, 10, 20);
brick.printValue("r", rgbCS[0], 1, 21);
brick.printValue("g", rgbCS[1], 2, 21);
brick.printValue("b", rgbCS[2], 3, 21);
brick.printValue("hue", hsvlCS[0], 5, 21);
brick.printValue("sat", hsvlCS[1], 6, 21);
brick.printValue("val", hsvlCS[2], 7, 21);
brick.printValue("light", hsvlCS[3], 8, 21);
brick.printValue("color", color, 10, 21);
}
control.pauseUntilTime(currTime, 10); // Ожидание выполнения цикла
}
Expand All @@ -58,7 +59,7 @@ function RgbToHsvlToColorConvert(debug: boolean = false) {
function Manipulator(motor: motors.Motor, state: ClawState, speed?: number, timeOut?: number) {
if (!speed) speed = 40; // Если аргумент не был передан, то за скорость установится значение по умолчанию
else speed = Math.abs(speed);
if (!timeOut) speed = 2000; // Если аргумент не был передан, то за максимальное время ожидания остановки устанавливается это значение
if (timeOut == undefined) speed = 2000; // Если аргумент не был передан, то за максимальное время ожидания остановки устанавливается это значение
else timeOut = Math.abs(timeOut);

motor.setBrake(true); // Устанавливаем ударжание мотора при остановке
Expand Down Expand Up @@ -129,10 +130,11 @@ function Main() { // Определение главной функции

sensors.SetColorSensorMaxRgbValues(L_COLOR_SEN, [273, 297, 355]);
sensors.SetColorSensorMaxRgbValues(R_COLOR_SEN, [230, 224, 178]);
sensors.SetColorSensorMaxRgbValues(CHECK_COLOR_CS, [363, 318, 371]);
sensors.SetColorSensorMaxRgbValues(CHECK_COLOR_CS, [354, 299, 354]);

CHASSIS_L_MOTOR.setInverted(true); CHASSIS_R_MOTOR.setInverted(false); // Установка реверсов в шасси
CHASSIS_L_MOTOR.setPauseOnRun(true); CHASSIS_R_MOTOR.setPauseOnRun(true); // Включаем у моторов ожидание выполнения
CHASSIS_L_MOTOR.setBrakeSettleTime(10); CHASSIS_R_MOTOR.setBrakeSettleTime(10); // Включаем у моторов ожидание выполнения

MANIPULATOR_MOTOR1.setInverted(true); MANIPULATOR_MOTOR2.setInverted(false); // Установить инверсию для манипулятора, если требуется
MANIPULATOR_MOTOR1.setBrake(true); MANIPULATOR_MOTOR2.setBrake(true); // Удержание моторов манипуляторов
Expand All @@ -157,42 +159,50 @@ function Main() { // Определение главной функции
Manipulator(MANIPULATOR_MOTOR2, ClawState.Open, 20, 1000);
});
// chassis.DistMove(10, 40, true);
chassis.PivotTurn(90, 50, WheelPivot.RightWheel);
motions.LineFollowToDistanceWithLeftSensor(HorizontalLineLocation.Outside, 700, AfterMotion.BreakStop, { speed: 50, Kp: 0.3 });
chassis.PivotTurn(35, 50, WheelPivot.LeftWheel);
chassis.PivotTurn(35, 50, WheelPivot.RightWheel);
motions.LineFollowToIntersection(AfterMotion.DecelRolling, { speed: 50, Kp: 0.3 });
chassis.PivotTurn(75, 40, WheelPivot.LeftWheel);
chassis.PivotTurn(75, 40, WheelPivot.RightWheel);
chassis.PivotTurn(90, 30, WheelPivot.RightWheel);
pause(250);
motions.LineFollowToDistanceWithLeftSensor(HorizontalLineLocation.Outside, 700, AfterMotion.DecelRolling, { speed: 30, Kp: 0.3, Kd: 1 });
pause(250);
chassis.PivotTurn(30, 30, WheelPivot.LeftWheel);
pause(100);
chassis.PivotTurn(30, 30, WheelPivot.RightWheel);
pause(100);
motions.LineFollowToIntersection(AfterMotion.DecelRolling, { speed: 40, Kp: 0.2, Kd: 1.5 });
pause(250);
chassis.PivotTurn(78, 30, WheelPivot.LeftWheel);
pause(100);
chassis.PivotTurn(78, 30, WheelPivot.RightWheel);
pause(100);
chassis.DistMove(240, 40, true);
// chassis.RampDistMove(240, 20, 30, 40);
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR1, ClawState.Open, 40);
});
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR2, ClawState.Open, 40);
});
pause(500);
chassis.DistMove(-550, 40, true);
// chassis.RampDistMove(-550, -20, -30, 40);
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR1, ClawState.Close, 40);
});
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR2, ClawState.Close, 40);
});
chassis.SpinTurn(-90, 40);
chassis.MoveToRefZone(SensorSelection.LeftOrRight, LogicalOperators.Less, 20, 0, -50, AfterMotion.BreakStop);
pause(500);
chassis.DistMove(-550, 30, true);
// chassis.RampDistMove(-550, -20, -30, 40);
chassis.SpinTurn(-90, 30);
pause(200);
chassis.MoveToRefZone(SensorSelection.LeftOrRight, LogicalOperators.Less, 20, 0, -30, AfterMotion.BreakStop);
levelings.LineAlignment(VerticalLineLocation.Behind, 1000);
chassis.DistMove(750, 50, true);
//chassis.RampDistMove(750, 20, 30, 60);
pause(200);
chassis.MoveToRefZone(SensorSelection.LeftOrRight, LogicalOperators.Less, 20, 0, -50, AfterMotion.BreakStop);
chassis.MoveToRefZone(SensorSelection.LeftOrRight, LogicalOperators.Less, 20, 0, -30, AfterMotion.BreakStop);
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR1, ClawState.Open, 40);
});
control.runInParallel(function () {
Manipulator(MANIPULATOR_MOTOR2, ClawState.Open, 40);
});
pause(100);
chassis.DistMove(60, 40, true);
chassis.SpinTurn(-90, 40);
motions.LineFollowToDistance(200, AfterMotion.NoStop);
motions.LineFollowToRightIntersection(HorizontalLineLocation.Inside, AfterMotion.Rolling);
motions.LineFollowToRightIntersection(HorizontalLineLocation.Inside, AfterMotion.DecelRolling, { speed: 40, Kp: 0.2, Kd: 1.5 });
}

Main(); // Вызов главной функции
1 change: 0 additions & 1 deletion motions.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,6 @@ namespace chassis {
// CHASSIS_MOTORS.setBrake(setBreak); // Удерживать при тормозе
// let mRotCalc = (dist / (Math.PI * WHEELS_D)) * 360; // Подсчёт по формуле
// CHASSIS_MOTORS.tank(speed, speed, mRotCalc, MoveUnit.Degrees); // Передаём команду моторам

CHASSIS_L_MOTOR.pauseUntilReady(); CHASSIS_R_MOTOR.pauseUntilReady(); // Ждём выполнения моторами команды
CHASSIS_L_MOTOR.setBrake(setBreak); CHASSIS_R_MOTOR.setBrake(setBreak); // Установить жёсткий тип торможения
const mRotCalc = (dist / (Math.PI * WHEELS_D)) * 360; // Расчёт угла поворота на дистанцию
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.21.2",
"version": "0.21.3",
"description": "Methods (blocks) for working with ev3 to participate in competitions.",
"dependencies": {
"ev3": "*",
Expand Down
14 changes: 7 additions & 7 deletions sensors.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,10 @@ namespace sensors {
export let bRefRawRightLineSensor: number; // Сырые значения на чёрном для правого датчика линии
export let wRefRawRightLineSensor: number; // Сырые значения на белом для правого датчика линии

export let maxRgbValuesCS1: number[]; // Максимальные значения RGB для датчика цвета в первом порту
export let maxRgbValuesCS2: number[]; // Максимальные значения RGB для датчика цвета во втором порту
export let maxRgbValuesCS3: number[]; // Максимальные значения RGB для датчика цвета в третьем порту
export let maxRgbValuesCS4: number[]; // Максимальные значения RGB для датчика цвета в четвёртом порту
export let maxRgbColorSensor1: number[]; // Максимальные значения RGB для датчика цвета в первом порту
export let maxRgbColorSensor2: number[]; // Максимальные значения RGB для датчика цвета во втором порту
export let maxRgbColorSensor3: number[]; // Максимальные значения RGB для датчика цвета в третьем порту
export let maxRgbColorSensor4: number[]; // Максимальные значения RGB для датчика цвета в четвёртом порту

/**
* Метод установки датчику линии сырых значений на чёрном и белом.
Expand Down Expand Up @@ -86,9 +86,9 @@ namespace sensors {
/* if (sensor == sensors.color1) maxRgbValuesCS1 = [maxRed, maxGreen, maxBlue];
else
*/
if (sensor == sensors.color2) maxRgbValuesCS2 = maxRgbArr;
else if (sensor == sensors.color3) maxRgbValuesCS3 = maxRgbArr;
else if (sensor == sensors.color4) maxRgbValuesCS4 = maxRgbArr;
if (sensor == sensors.color2) maxRgbColorSensor2 = maxRgbArr;
else if (sensor == sensors.color3) maxRgbColorSensor3 = maxRgbArr;
else if (sensor == sensors.color4) maxRgbColorSensor4 = maxRgbArr;
}

/**
Expand Down
2 changes: 1 addition & 1 deletion turns.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,7 @@ namespace chassis {
return;
}
CHASSIS_L_MOTOR.pauseUntilReady(); CHASSIS_R_MOTOR.pauseUntilReady(); // Ждём выполнения моторами команды ??????
//CHASSIS_MOTORS.setBrake(true); // Удерживать при тормозе
// CHASSIS_MOTORS.setBrake(true); // Удерживать при тормозе
CHASSIS_L_MOTOR.setBrake(true); CHASSIS_R_MOTOR.setBrake(true); // Удерживать при тормозе
let calcMotRot = ((deg * WHEELS_W) / WHEELS_D) * 2; // Расчёт значения угла для поворота
if (wheelPivot == WheelPivot.LeftWheel) {
Expand Down

0 comments on commit f7df289

Please sign in to comment.