diff --git a/lw.ts b/lw.ts index 582d9c6..9e5e4a7 100644 --- a/lw.ts +++ b/lw.ts @@ -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(); // Сброс ПИД регулятора @@ -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); @@ -196,7 +196,7 @@ namespace motions { control.pauseUntilTime(currTime, 10); // Ожидание выполнения цикла } music.playToneInBackground(262, 300); // Издаём сигнал завершения - chassis.ActionAfterMotion(lineFollow2SensorSpeed, actionAfterMotion); // Действие после алгоритма движения + chassis.ActionAfterMotion(lineFollowLeftSensorSpeed, actionAfterMotion); // Действие после алгоритма движения } /** diff --git a/main.ts b/main.ts index 79203d8..3aba359 100644 --- a/main.ts +++ b/main.ts @@ -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; // Ссылка на объект мотора манипулятора @@ -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); // Ожидание выполнения цикла } @@ -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); // Устанавливаем ударжание мотора при остановке @@ -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); // Удержание моторов манипуляторов @@ -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(); // Вызов главной функции \ No newline at end of file diff --git a/motions.ts b/motions.ts index 0cdbfba..2f5e01d 100644 --- a/motions.ts +++ b/motions.ts @@ -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; // Расчёт угла поворота на дистанцию diff --git a/pxt.json b/pxt.json index c60bddb..b07a4f6 100644 --- a/pxt.json +++ b/pxt.json @@ -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": "*", diff --git a/sensors.ts b/sensors.ts index 97809c6..335e411 100644 --- a/sensors.ts +++ b/sensors.ts @@ -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 для датчика цвета в четвёртом порту /** * Метод установки датчику линии сырых значений на чёрном и белом. @@ -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; } /** diff --git a/turns.ts b/turns.ts index 4e03be0..224b9c4 100644 --- a/turns.ts +++ b/turns.ts @@ -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) {