Зайцев Егор Владимировичnti-contest.ru › wp-content › uploads › res-2018...

Работа победителя заключительного этапа

командной инженерной олимпиады школьников

Олимпиада Национальной технологической инициативы

Профиль «АВТОНОМНЫЕ ТРАНСПОРТНЫЕ СИСТЕМЫ»

Зайцев Егор Владимирович

Класс: 10

Школа: МАОУ школа-интернат лицей-

интернат г. Калининград

Уникальный номер участника: 428114

Город: Калининград

Регион: Калининградская

область

Команда на заключительном

этапе: 47ds-ds46

Результаты заключительного этапа:

Расшифровка индивидуальной части:

* Оценка формируется следующим образом:

Компонента командного этапа:

Командный результат - сумма баллов команды по итогам командной части заключительного

этапа, максимальный балл 210 (сумма баллов по итогам выполнения задач подтеков

«Marinet», «Autonet», «Aeronet», «Общие задачи полигона АТС»);

Нормированный командный балл - командный результат, поделенный на максимальный

балл (210);

Коэффициент 60% - нормированный командный балл, умноженный на 60% (см. Материалы

заданий олимпиады школьников за 2017/18 учебный год, раздел 3.4 Критерий определения

победителей и призеров заключительного этапа)

Компонента индивидуальной части:

Индивидуальный тур по физике нормированный - сумма баллов за решение задач по физике,

деленная на 80 (максимальный балл);

Индивидуальный тур по информатике нормированный - сумма баллов за решение задач по

информатике, деленная на 100 (максимальный балл);

Коэффициент 40% - сумма нормированных индивидуальных баллов, умноженная на 40%

(см. 3.4 Критерий определения победителей и призеров заключительного этапа);

Итог - сумма взвешенных оценки за командную и индивидуальную части заключительного этапа

(«коэффициент 60%» и «коэффициент 40%»), умноженная на 100.

Индивидуальная часть Персональный лист участника с номером 428114:

Физика

Информатика

Задача 1.1

Код программы на языке C++: #include <iostream>

#include <fstream>

#include <string>

#include <vector>

#include <math.h>

#include <cmath>

using namespace std;

struct pt {

int x, y;

};

const double EPS = 1E-9;

inline int det (int a, int b, int c, int d) {

return a * d - b * c;

}

inline bool between (int a, int b, double c) {

return min(a,b) <= c + EPS && c <= max(a,b) + EPS;

}

inline bool intersect_1 (int a, int b, int c, int d) {

if (a > b) swap (a, b);

if (c > d) swap (c, d);

return max(a,c) <= min(b,d);

}

bool intersect (pt a, pt b, pt c, pt d) {

int A1 = a.y-b.y, B1 = b.x-a.x, C1 = -A1*a.x - B1*a.y;

int A2 = c.y-d.y, B2 = d.x-c.x, C2 = -A2*c.x - B2*c.y;

int zn = det (A1, B1, A2, B2);

if (zn != 0) {

double x = - det (C1, B1, C2, B2) * 1. / zn;

double y = - det (A1, C1, A2, C2) * 1. / zn;

return between (a.x, b.x, x) && between (a.y, b.y, y)

&& between (c.x, d.x, x) && between (c.y, d.y, y);

}

else

return det (A1, C1, A2, C2) == 0 && det (B1, C1, B2, C2) == 0

&& intersect_1 (a.x, b.x, c.x, d.x)

&& intersect_1 (a.y, b.y, c.y, d.y);

}

int main()

{

int n, m;

cin >> n >> m;

pt P[n];

pt M[m];

for(int i = 0 ; i < n;i++){

cin >> P[i].x >> P[i].y;

}

for(int i = 0 ; i < m;i++){

cin >> M[i].x >> M[i].y;

}

if(intersect(P[0],P[1],M[0],M[1])){

cout<<"Yes";

}else{

cout<<"No";

}

return 0;

}

Программа верно решает задачу (5 баллов).

Командная часть

Результаты были получены в рамках выступления команды: 47ds-ds46

Личный состав команды:

● Гринченко Кирилл

● Дубынин Алексей

● Зайцев Егор

● Воркожоков Денис

Фото команды с собранными устройствами:

Рис.1 - Команда с тремя устройствами в сборе

Рис.2 - Команда с тремя устройствами в сборе

Баллы командного этапа:

Маринет

Автонет

Аэронет

Общие задачи полигона АТС

Решения командного этапа:

Маринет

Фото устройства:

Код программы на языке С++, обеспечивающий решение задач подтрека:

«marinet.ino»

#define __AVR_ATmega168__ 1

#include <ros.h>

#include <geometry_msgs/Pose2D.h>

#include <IRLibDecodeBase.h>

#include <IRLib_P02_Sony.h>

#include <IRLibCombo.h>

#include <Wire.h>

#include <VL53L0X.h>

#include <IRLibRecv.h>

IRrecv myReceiver(A3); //Пин куда подключен сигнал приемника

IRdecode myDecoder;

int max_speed = 100;

#define IN1 10

#define IN2 11

#define IN3 5

#define IN4 9

VL53L0X lidar;

int lidarMax = 800;

int lidarTemp;

int lidarDist = lidarMax;

String str;

#define MAX_DISTANCE 200

#define FRsonar 8 // Передний правый

#define RRsonar 12 // Задний правый

#define FLsonar A4 // Передний левый

#define RLsonar A5 // Задний левый

#define Vin A0 // Контроль напряжения

#define Mins 50 // Минимальное расстояние до борта

double distFR;

double distRR;

double distFL;

double distRL;

float cord[3];

float Voltage;

char IK = 'N';

void setup() {

delay(3000);

Serial.begin(57600);

Wire.begin();

Serial1.begin(57600);

pinMode(13, OUTPUT);

pinMode(A1, OUTPUT);

pinMode(A2, OUTPUT);

myReceiver.enableIRIn(); // Запуск ресивера

//DC motor pins

pinMode(IN1, OUTPUT);




lidar.init();

lidar.setTimeout(200);

lidar.startContinuous();

IK = Receiver();

while(IK == 'N') IK = Receiver();

Cord();

vertushka(-90.0);

Cord();

if (IK == 'L'){

goHome(0.85, cord[1], -90.0, false);

vertushka(-35.0);

delay(2000);

goHome(cord[0], 2.44, -3.0, true);

Cord();

vertushka(55.0);

delay(2000);

Cord();

goHome(0.22, cord[1], 88.0, false);

}

else{

goHome(0.9, cord[1], -85.0, false);

vertushka(10.0);

delay(200);

goHome(cord[0], 0.9, 45, true);

vertushka(0.0);

delay(200);

goHome(cord[0], 2.44, -2.0, true);

Cord();

vertushka(65.0);

delay(200);

Cord();

goHome(0.22, cord[1], 80.0, false);

}

goMotor(0,0);

digitalWrite(A1, HIGH);

digitalWrite(A2, HIGH);

goMotor(0,0);

//////////////////////////////////

}

void loop() {

//goMotor(117, 120);

}

void vertushka(double theta) {

int dt = 2;

while (abs(theta - cord[2]) >= dt)

{

Cord();

if (cord[2] < theta)

goMotor(-100, 80);

else

goMotor(80, -100);

dt = 20;

delay(10);

}

}

void goHome(double X, double Y, double angle, bool y){

double a[2] = {X,Y};

Cord();

while (abs(cord[y] - a[y]) > 0.04){

if (cord[2] - angle >= 0.5){

goMotor(100 + int(ceil(abs((cord[2] - angle))) * 3.3), 100);

}

else

if (cord[2] - angle <= -0.5){

goMotor(100, 100 + int(ceil(abs((cord[2] - angle))) * 4.0));

}

else

goMotor(96, 100);

Cord();

}

goMotor(-150, -150);

delay(1450);

goMotor(0,0);

Cord();

}

void goMotor(int lft, int rgt ) {

if (lft > 0) {

analogWrite(IN2, lft);

digitalWrite(IN1, 0);

} else {

analogWrite(IN2, 255 - abs(lft));


}

if (rgt < 0) {

analogWrite(IN3, 255 - abs(rgt));


} else {

analogWrite(IN3, rgt);


}

}

void SonarScan ()

{

distFL = getSonar(FLsonar);

Serial.print("FL:");

Serial.println(distFL);

distRL = getSonar(RLsonar);

Serial.print("RL:");

Serial.println(distRL);

distFR = getSonar(FRsonar);

Serial.print("FR:");

Serial.println(distFR);

distRR = getSonar(RRsonar);

Serial.print("RR:");

Serial.println(distRR);

}

double getSonar(int SonarPin) {

pinMode(SonarPin, OUTPUT);

digitalWrite(SonarPin, HIGH);

delayMicroseconds(10);

digitalWrite(SonarPin, LOW);

pinMode(SonarPin, INPUT);

return pulseIn(SonarPin, HIGH, 15000.0) / 58.0;

}

char Receiver() {

if (myReceiver.getResults())

{

myDecoder.decode(); // Декодируем...

if (myDecoder.protocolNum == SONY) {

Serial.println(myDecoder.value, HEX);

}

myReceiver.enableIRIn(); // Перезапускаем приемник

if (myDecoder.value == 0x60) {

digitalWrite(A2, HIGH); // Правый сигнальный светодиод

digitalWrite(A1, LOW);

return 'R';

}


digitalWrite(A1, HIGH); // Левый сигнальный светодиод

digitalWrite(A2, LOW);

return 'L';

}

return 'N';

}

return 'N';

}

void GetV() {

double val = analogRead(Vin);

val = ((val * 4.9) / 1024.0) * 2.0;

Serial.print("volts: "); Serial.println(val);

}

void Cord() {

while (Serial1.available() > 0) {

char recieved = Serial1.read();

str += recieved;

if (recieved == '\n') {

//Serial.print(str);

int index = str.indexOf(';');

int secondIndex = str.indexOf(';', index + 1);

// put your main code here, to run repeatedly:

cord[0] = str.substring(0, index).toFloat();

cord[1] = str.substring(index + 1, secondIndex).toFloat();

cord[2] = str.substring(secondIndex + 1).toFloat();

Serial.println(cord[0]);



Serial.println("\n");

str = "";

}

}

}

Автонет


Код программы на языках С++/Python, обеспечивающий решение задач подтрека:

«check.ino»

// получает координату

#include <Servo.h>

#define Lsonar 34 // Левый дальномер

#define Rsonar 36 // Правый дальномер

#define Csonar 32 // Центральный дальномер


#define D_PIN 8 // сервопривод поворота

#define M_PIN 9 // драйвер моторов

#define dirR 70 // вправо

#define dirF 95 // прямо

#define dirL 110 // влево

#define SpeedF 1400

#define SpeedB 1590

#define x_ex 100

#define y_ex 300

double distC = 0;

double distL = 0;

double distR = 0;

unsigned long int lastTime = millis();

unsigned long int lastTimeGet = millis();

bool stopped = true; // флаг для быстрой остановки

bool flag = true; //переключение мотора(вперед)

char c = 'F'; // символ для управления направлением сервы

bool start = true; // подключение движков, флаг

bool sign = false; // L - false, R - true

bool last_sign = false; // последний сохраненный

int x = 5; // координаты и угол

int y = 5;

int angle = -90;

int angleServoint = 95;

float angleServofl = 95.0;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);


myservo.attach(M_PIN);

myservo2.attach(D_PIN);

setupIR();

STOP();

delay(500);

}

void loop() {

if (start) {

STOP();

delay(700);

start = false;

back();

delay(500);

forward();

delay(100);

}

//distC = getSonar(Csonar);

//distR = getSonar(Rsonar);

//distL = getSonar(Lsonar);

if (lastTime + 100 < millis()) {

GetCoordinate();

Serial.println(x);

lastTimeGet = millis();

}

//if (Serial.available()) c = Serial.parseInt();

// Остановка по препятствию

//Serial.println(distC);

//if (distC < 20) fastSTOP();

//Управление по IR

//compIR();

// if (sign){

// forward();

// delay(100);

// }

// else if (!sign){

// STOP();

// delay(100);

// }


int angleB = atan2(abs(x_ex - x), abs(y_ex - x)); // угол между траекторией и осью

int angleC = 0; // угол поворотаА

if (x <= x_ex && y <= y_ex && angleB > -angle) {

angleC = angleB + angle;

}

else if (x <= x_ex && y <= y_ex && angleB < -angle) {

angleC = -angleB - angle ;

}

else if (x >= x_ex && y <= y_ex && angleB > angle) {

angleC = angleB - angle;

}

else if (x >= x_ex && y <= y_ex && angleB < angle) {

angleC = - angleB + angle;

}

int x_begin = x;

int y_begin = y;

int D = (x - x_ex) * (y_begin - y_ex) - (y - y_ex) * (x_begin - x_ex);

if (D > 0) {

//цикл смещения влево

angleServofl = angleServoint - angle * 0.05;

myservo2.write(int(angleServofl));

}

else if (D < 0) {

//цикл смещения вправо

}

else if (D == 0) {

//просто ехать вперед

}

}

//c = 'F';

//DIRECTION(c);

forward();

delay(10);

}

void DIRECTION(char c) {

if (c == 'L') myservo2.write(dirL);

else if (c == 'R') myservo2.write(dirR);

else myservo2.write(dirF);

}

void changeDir() {

myservo.writeMicroseconds(SpeedF);

delay(100);

myservo.writeMicroseconds(1500);

delay(100);


//delay(100);

flag = false;

Serial.println("Dir changed");

}

void forward() {

if (flag) changeDir();


Serial.println("forward");

delay(10);

}

void back() {

myservo.writeMicroseconds(SpeedB);

Serial.println("back");

flag = true;

delay(10);

}

void STOP() {


Serial.println("STOP");

flag = true;

}

int getSonar(int SonarPin) {






return pulseIn(SonarPin, HIGH, 15000) / 58;

}

void GetCoordinate() {

if (Serial3.available()) {

if (ParseCoordinate()) {

PrintCoordinate();

Serial.println("Координаты успешно получены");

}

else {

Serial.println("Не видно маркер, координаты остались прежними");

}

}

else {

Serial.println("Serial is empty");

}

}

bool ParseCoordinate()

{

//delay(20);

String str = Serial3.readStringUntil('\n');

// Serial.print("str : ");

Serial.println(str);

if (str.length() < 16) {

//str = "00/123/456/-179"; // пример строки

if (str.substring(0, 2).toInt() == -5)

{ Serial.println("Marker not found");

return false;

}

int x_new = str.substring(3, 6).toInt();

int y_new = str.substring(7, 10).toInt();

int angle_new = str.substring(11, str.length()).toInt();

if (x_new * y_new != 0) {

x = x_new;

y = y_new;

angle = angle_new;

}

return true;

}

return false;

}

void PrintCoordinate()

{ Serial.println();

Serial.print("X=");

Serial.print(x);

Serial.print(" :: Y=");

Serial.print(y);

Serial.print(" :: Angle=");

Serial.println(angle);

}

«coord.ino»


#include <Servo.h>










#define SpeedF 1400

#define SpeedB 1590

#define x_ex 100

#define y_ex 300

double distC = 0;

double distL = 0;

double distR = 0;










int y = 5;

int angle = -90;



Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);




setupIR();

STOP();

delay(500);

}

void loop() {

if (start) {

STOP();

delay(700);

start = false;

back();

delay(500);

forward();

delay(100);

}





GetCoordinate();

Serial.println(x);

lastTimeGet = millis();

}






//compIR();

// if (sign){

// forward();

// delay(100);

// }

// else if (!sign){

// STOP();

// delay(100);

// }


int angleB = atan2(abs(x_ex - x), abs(y_ex - x)); // угол между траекторией и осью

int angleC = 0; // угол поворотаА

if (x <= x_ex && y <= y_ex && angleB > -angle) {

angleC = angleB + angle;

}

else if (x <= x_ex && y <= y_ex && angleB < -angle) {

angleC = -angleB - angle ;

}

else if (x >= x_ex && y <= y_ex && angleB > angle) {

angleC = angleB - angle;

}

else if (x >= x_ex && y <= y_ex && angleB < angle) {

angleC = - angleB + angle;

}

int x_begin = x;

int y_begin = y;

int D = (x - x_ex) * (y_begin - y_ex) - (y - y_ex) * (x_begin - x_ex);

if (D > 0) {

//цикл смещения влево

angleServofl = angleServoint - angle * 0.05;

myservo2.write(int(angleServofl));

}

else if (D < 0) {

//цикл смещения вправо

}

else if (D == 0) {

//просто ехать вперед

}

}

//c = 'F';

//DIRECTION(c);

forward();

delay(10);

}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = false;


}

void forward() {




delay(10);

}

void back() {



flag = true;

delay(10);

}

void STOP() {



flag = true;

}








}




PrintCoordinate();


}

else {


}

}

else {


}

}


{

//delay(20);








return false;

}





x = x_new;

y = y_new;

angle = angle_new;

}

return true;

}

return false;

}


{ Serial.println();

Serial.print("X=");

Serial.print(x);


Serial.print(y);



}

«IR_example.ino»

//https://github.com/cyborg5/IRLib2




IRdecode myDecoder;


IRrecv myReceiver(11); //Пин куда подключен сигнал приемника

void setupIR() {


Serial.println(("Ready to receive IR signals"));

}

char getIR() {


{


if (myDecoder.protocolNum == SONY) {

Serial.println(myDecoder.value);

}


if (myDecoder.value == 48) { //0x50

Serial.println('L');

return 'L';

}

if (myDecoder.value == 64) { //0x60

Serial.println('R');

return 'R';

}

else return 'e';

}

}

void compIR(){

char ir_char = getIR();

if (ir_char == 'L') sign = false;

else if (ir_char == 'R') sign = true;

//if (sign != last_sign) last_sign = sign;

}

«coord.ino»

Файл пуст.

«direct_move.ino»

//2с вперед, 2с назад

#include <Servo.h>






int flag = 1; //переключение мотора(вперед)

char c = 'F';

int start = 1;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);



STOP();

delay(1000);

}

void loop() {

if(start == 1) {

STOP();

delay(700);

start = 0;

}


c = 'F';

DIRECTION(c);

forward();

delay(2000);

back();

delay(2000);

}





}

void changeDir() {


delay(100);


delay(100);


delay(100);

flag = 0;


}

void forward() {

if (flag == 1) changeDir();



delay(10);

}

void back() {



flag = 1;

delay(10);

}

void STOP() {



flag = 1;

}

«eight_move.ino»

//2с вперед, 2с назад

#include <Servo.h>







char c = 'F';

int start = 1;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);



STOP();

delay(1000);

}

void loop() {

if(start == 1) {

STOP();

delay(700);

start = 0;

}


c = 'R';

DIRECTION(c);

forward();

delay(2000);

c = 'L';

DIRECTION(c);

delay(2000);

}





}

void changeDir() {


delay(100);


delay(100);


delay(100);

flag = 0;


}

void forward() {




delay(10);

}

void back() {



flag = 1;

delay(10);

}

void STOP() {



flag = 1;

}

«line_moving//////////////////////////////////////

// клешня разжата - red //

// клешня сжата - green //

// //

// перелет - purple //

// взлет - r-g-b //

// зависание - white //

// преземление - сюрприз //

//////////////////////////////////////

#include "Adafruit_NeoPixel.h"

#define LED_COUNT 30

#define LED_PIN 10

Adafruit_NeoPixel strip = Adafruit_NeoPixel(LED_COUNT, LED_PIN, NEO_GRB +

NEO_KHZ800);

void setupLED() {

strip.begin();

NOzahvatLED();

}

void loopLED()

{

}

void zahvatLED() { // клешня сжата - green

for (int i = 0; i < LED_COUNT - 1; i++)

{

strip.setPixelColor(i, strip.Color(0, 255, 0));

}

strip.show();

}

void NOzahvatLED() { // клешня разжата - red


{


}

strip.show();

}

void zavisanieLED() { // зависание - white


{


}

strip.show();

}

void pereletLED() { // перелет - purple


{


}

strip.show();

}

void vzletLED() { // взлет - r-g-b

static byte c[5] = {255, 0, 0, 255, 0};

for (int i = 0; i < LED_COUNT - 1; i++) {

byte j = i % 3;

strip.setPixelColor(i, strip.Color(c[j], c[j + 1], c[j + 2]));

}

strip.show();

}

void posadkaLED() { // посадка - переливание

lastTime = millis();

while (lastTime + 5000 > millis() ) {

for (byte k = 0; k < 3; k++) {

static byte c[7] = {255, 0, 0, 255, 0, 0, 255};

for (byte i = 0; i < LED_COUNT - 1; i++) {

int j = i % 3;

strip.setPixelColor(i, strip.Color(c[j + k], c[j + k + 1], c[j + k + 2]));

}

strip.show();

}

}

}

void waitLED() { // ожидание - переливание

static int c[7] = {200, 200, 0, 200, 200, 0, 200};


int j = i % 3;


strip.show();

}

}»


#include <Servo.h>










#define SpeedF 1400

#define SpeedB 1590

#define x_ex 100

double distC = 0;

double distL = 0;

double distR = 0;








int y = 5;

int angle = -90;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);




setupIR();

STOP();

delay(500);

}

void loop() {

if (start) {

STOP();

delay(700);

start = false;

back();

delay(500);

forward();

delay(100);

}




Serial.println(x);

GetCoordinate();






//compIR();

// if (sign){

// forward();

// delay(100);

// }

// else if (!sign){

// STOP();

// delay(100);

// }

if (x > x_ex) {

myservo2.write(dirF + 10);

}

else if (x < x_ex) {

myservo2.write(dirF - 10);

}

else {

myservo2.write(dirF);

}

//c = 'F';

//DIRECTION(c);

forward();

}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = false;


}

void forward() {




delay(10);

}

void back() {



flag = true;

delay(10);

}

void STOP() {



flag = true;

}








}




PrintCoordinate();


}

else {


}

}

else {


}

}


{

//delay(20);








return false;

}





x = x_new;

y = y_new;

angle = angle_new;

}

return true;

}

return false;

}


{ Serial.println();

Serial.print("X=");

Serial.print(x);


Serial.print(y);



}

«prilipala.ino»

// следует за препятствием в 30см

#include <Servo.h>



#define Csonar 32 // Центральный дальнчомер







#define SpeedF 1400

#define SpeedB 1590

double distC = 0;

double distL = 0;

double distR = 0;

int stopped = 1;


char c = 'F';

int start = 1;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);



STOP();

delay(500);

}

void loop() {

if (start == 1) {

STOP();

delay(700);

start = 0;

back();

delay(500);

forward();

delay(100);

}

distC = getSonar(Csonar);




c = 'F';

DIRECTION(c);

Serial.println(distC);

if (distC < 1 && stopped) forward();

else if (distC < 20 && stopped) fastSTOP();

}





}

void changeDir() {


delay(50);


delay(50);


//delay(100);

flag = 0;


}

void forward() {




//delay(10);

}

void back() {



flag = 1;

//delay(10);

}

void STOP() {



flag = 1;

}

void fastSTOP() {

if (stopped == 1) {


delay(50);

stopped = 0;

}



flag = 1;

}








}

«with_get_coord.ino»


#include <Servo.h>




IRdecode myDecoder;












#define SpeedF 1400

#define SpeedB 1590

double distC = 0;

double distL = 0;

double distR = 0;

int stopped = 1; // флаг для быстрой остановки



int start = 1; // подключение движков, флаг


int y = 5;

int angle = -90;

char stopSignal = 'L'; // сигнал для остановки

char starSignal = 'R'; // сигнал для езды

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);




setupIR();

STOP();

delay(500);

}

void loop() {

if (start == 1) {

STOP();

delay(700);

start = 0;

back();

delay(500);

forward();

delay(100);

}




GetCoordinate();


c = 'F';

DIRECTION(c);




// Работа с ИК

// if (getIR() == stopSignal) {

// STOP();

// }

// else if (getIR() == startSignal) {

// forward();

// }

forward();

delay(100);

}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = 0;


}

void forward() {




delay(10);

}

void back() {



flag = 1;

delay(10);

}

void STOP() {



flag = 1;

}








}




PrintCoordinate();


}

else {


}

}

else {


}

}


{

//delay(20);



// Serial.println(str);




return false;

}

x = str.substring(3, 6).toInt();

y = str.substring(7, 10).toInt();

angle = str.substring(11, str.length()).toInt();

return true;

}

void PrintCoordinate(void)

{ Serial.println();

Serial.print("X=");

Serial.print(x);


Serial.print(y);



}

«with_get_IR.ino»

// следует за препятствием в 30см

#include <Servo.h>










#define SpeedF 1380

#define SpeedB 1590

double distC = 0;

double distL = 0;

double distR = 0;

char sign = 'L';

char last_sign = 'L';

int stopped = 1;


char c = 'F';

int start = 1;

char stopSignal = 'L';

char startSignal = 'R';

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(57600);



setupIR();

STOP();

delay(500);

}

void loop() {

if (start == 1) {

STOP();

delay(700);

start = 0;

back();

delay(500);

forward();

delay(50);

STOP();

}

distC = getSonar(Csonar);

distR = getSonar(Rsonar);

distL = getSonar(Lsonar);


c = 'F';

DIRECTION(c);

sign = getIR();

Serial.println(sign);

if (sign != last_sign && sign != 'e'){

last_sign = sign;

}

if (last_sign == stopSignal) {

STOP();

delay(100);

}

else if (last_sign == startSignal) {

forward();

delay(100);

}

//delay(100);

}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = 0;


}

void forward() {




delay(10);

}

void back() {



flag = 1;

delay(10);

}

void STOP() {



flag = 1;

}

void fastSTOP() {

if (stopped == 1) {


delay(50);

//stopped = 0;

}



flag = 1;

}








}

«zadanie_4.ino»

//движение по светофору

#include <Servo.h>








double distC;


char c = 'F';

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);



STOP();

delay(500);

}

void loop() {


c = 'F';

DIRECTION(c);

char f = getIR();

if (f == '1'){

forward();

}

else if (f == '0'){

STOP();

}

else if (f == 'e'){

Serial.println("Not found");

}

}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = 0;


}

void forward() {




delay(10);

}

void back() {



flag = 1;

delay(10);

}

void STOP() {



flag = 1;

}








}

«motor.ino»

#include <Servo.h>

#include <NewPing.h>

#define TRIGGER_PIN 15

#define ECHO_PIN 16


void STOP();

void forward(int);

void back();

void line();

int dir1;

bool left, center, right;

int Speed = 50;

byte dir = 90; // 90 - прямо, 60 - вправо, 120 - влево

unsigned long int oldtime;

Servo myservo;

Servo myservo2;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup () {

Serial.begin(9600);

for(int i = 43; i<=45; i++) pinMode(i,INPUT);

myservo.attach(9); // 10 пин, драйвера

myservo2.attach(3); // 9 пин, поворот

}

void loop(){

Speed = 10;

line();

int dist = sonar.ping_cm();

// if (dist < 30) back();

// else if (dist < 40) STOP();

// else

forward(Speed);

}

void STOP(){

myservo.write(90);


delay(50);

}

void forward(int sp){

sp = map(sp, 0, 100, 104, 140);

myservo.write(sp);

// Serial.println("forward"+String(sp));

delay(10);

}

void back(){

myservo.write(70);


delay(10);

}

void line(){

left = digitalRead(45);

center = digitalRead(44);

right = digitalRead(43);

if(center) dir=90;

else if(left) dir1=2;

else if(right) dir1=-2;

if(oldtime+1<millis()){oldtime = millis(); if(dir1==-2 && dir>60 || dir1==2 && dir<120){dir =

dir+dir1; myservo2.write(dir);}

else myservo2.write(dir);

}

}

«motor_rabotaet.ino»

#include <Servo.h>

#define M_PIN 9

#define D_PIN 10

void STOP();

void forward(int);

void back();

int sp = 1500;


int c = 0; // 0 - старт/стоп, 1 - вперед, 2 - назад

int flag = 1; // запуск мотора вперед

Servo myservo;

Servo myservo2;

void setup() {

// put your setup code here, to run once:

Serial.begin(9600);




delay(600);

}

void loop() {

// put your main code here, to run repeatedly:

if(Serial.available()){

sp = Serial.parseInt();

}

forward(sp);

delay(10);

}

void STOP(){



delay(50);

}


//byte sp1;

//sp = map(sp1, 0, 100, 104, 140);

myservo.writeMicroseconds(sp);

//myservo.writeMicroseconds(sp);


Serial.print(sp);

delay(10);

}

void changeDir(){


delay(100);


delay(100);


//delay(100);


}

void back(){



delay(10);

}

void start(){


Serial.println("start");

}

«motor_test.ino»

#include <Servo.h>

#include <NewPing.h>

#define TRIGGER_PIN 15

#define ECHO_PIN 16


#define M_PIN 9

void STOP();

void forward(int);

void back();

int Speed = 50;


Servo myservo;

Servo myservo2;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup () {

Serial.begin(9600);

myservo.attach(9); // 10 пин, драйвера

myservo2.attach(3); // 9 пин, поворот

}

void loop() {

if(Serial.available()){

Speed = Serial.parseInt();

}

myservo2.write(dir); // поворот

int dist = sonar.ping_cm();

Serial.println(dist);

if (dist < 30) back();

else if (dist < 40) STOP();

else forward(Speed);

delay(10);

Serial.println("------"+String(Speed));

}

void STOP(){



delay(50);

}


//byte sp1;

//sp = map(sp1, 0, 100, 104, 140);

myservo.writeMicroseconds(sp);

Serial.println("forward"+String(sp));

delay(10);

}

void back(){



delay(10);

}

«copy.ino»


#include <Servo.h>







#define dirR 75// вправо



#define SpeedF 1390

#define SpeedB 1590

#define x_ex 90

#define y_ex 117

#define x_st1 40 // начало поворота

#define y_st1 10

#define x_st2 95 // конец поворота

#define y_st2 96

#define x_st3 81 // конец выпрямления, поворот

#define y_st3 175

#define x_st4 65// поворот перед портом

#define y_st4 172

#define x_st5 36 // порт

#define y_st5 246

#define x_end 45

#define y_end 250

//координаты груза: 40/253/40 (angle)



double distC = 0;

double distL = 0;

double distR = 0;

bool proverka = false;









char stopSignal = 'L';

char startSignal = 'R';


int y = 5;

int angle = -90;

Servo myservo;

Servo myservo2;

void setup() {

Serial.begin(9600);




setupIR();

STOP();

delay(500);

}

void loop() {

if (start) {

STOP();

delay(700);

start = false;

back();

delay(500);

STOP();

GetCoordinate();

// 1 участок, по прямой

delay(500);

GetCoordinate();

delay(500);

GetCoordinate();

Serial.println("ANGLE");


start = 0;

}

proverka = compIR();

if (proverka){

if (angle > -87) {

//angleServofl = angleServoint - angle * 0.05;


Serial.println("left");

}

else if (angle < -90) {



Serial.println("right");

}


delay(300);

forward();

delay(100);

while (x < x_st1) {

GetCoordinate();

if (angle > -87) {



Serial.println("left");

}

else if (angle < -90) {



Serial.println("right");

}


}

//2 участок - окружность

GetCoordinate();

myservo2.write(dirL);

while (y < y_st2) {

GetCoordinate();

forward();

}

//myservo2.write(dirF-10);

// 3 участок - выпрямление


//int angleB = atan2(abs(x - x_st3), abs(y - y_st3));

while (y < y_st4) {

forward();

GetCoordinate();


}

// 4 участок - поворот к порту

while (x > x_end-10) {

GetCoordinate();

myservo2.write(dirL);

}



STOP();

}

//forward();

//GetCoordinate();

// if (lastTimeGet + 50 < millis()) {

// GetCoordinate();

// lastTimeGet = millis();

// }

//delay(20);

// if (lastTime + 50 < millis()) {

// //int deltax = abs(x - x_ex);

// //int deltay = abs(y - y_ex);

// //int angleB = atan2(deltax, deltay); // угол отклонения

// //int angleA = angleB + angle; // угол поворота

//

// if (x > x_ex + 1) {

// //angleServofl = angleServoint - angle * 0.05;

// myservo2.write(dirF + 15);

// Serial.println("left");

// }

// else if (x < x_ex - 1) {

// //angleServofl = angleServoint - angle * 0.05;

// myservo2.write(dirF - 15);

// Serial.println("right");

// }

// }


}





}

void changeDir() {


delay(100);


delay(100);


//delay(100);

flag = false;


}

void forward() {



//Serial.println("forward");

delay(10);

}

void back() {



flag = true;

delay(10);

}

void STOP() {



flag = true;

}








}




//PrintCoordinate();

//Serial.println("Координаты успешно получены");

}

else {

//Serial.println("Не видно маркер, координаты остались прежними");

}

}

else {

//Serial.println("Serial is empty");

}

}


{

//delay(20);


//String str = Serial3.readString();


//Serial.println("HERE");






return false;

}




x = x_new;

y = y_new;

angle = angle_new;

return true;

}

return false;

}


{ Serial.println();

Serial.print("X=");

Serial.print(x);


Serial.print(y);



}

«defs.py» (Дополнительное задание)

# -*- coding: utf-8 -*-

# распознавание дорожных знаков

import cv2 as cv

import os

import numpy as np

import time

from picamera import PiCamera

from picamera.array import PiRGBArray

print (cv.__file__)

import math

# from matplotlib import pyplot as plt

# пороговые значения, из hsvSettings

lower = np.array([0, 119, 107])

upper = np.array([255, 255, 255])

camera = PiCamera()

camera.resolution = (640, 480)

camera.framerate = 25

rawCapture = PiRGBArray(camera, size=(640, 480))

#time.sleep(0.1)

def imread():

img = cv.imread('C:/Users/Denis/Documents/drive/Google Drive/Projects/Computer

Vision/sign/img/ph2.jpg',

cv.IMREAD_COLOR)

return img

def pi_imread():

camera.capture(rawCapture, 'bgr', use_video_port=True)

img = rawCapture.array

rawCapture.truncate(0)

return(img)

def img_tr(img):

hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)

hsv = cv.blur(hsv, (5, 5)) # размытие

th = cv.inRange(hsv, lower, upper) # маска области

th = cv.erode(th, None, iterations=2) # размывание (сужение)

th = cv.dilate(th, None, iterations=6) # растягивание (расширение)

_, contours, hierarchy = cv.findContours(th, cv.RETR_TREE,

cv.CHAIN_APPROX_SIMPLE) # поиск контуров

for cnt in contours:

c = sorted(contours, key=cv.contourArea, reverse=True)[0] # сортировка по площади

rect = cv.minAreaRect(c) # граница box

# cv.drawContours(img, contours, -1, (0, 255, 0), 2)

# print(rect)

box = cv.boxPoints(rect)

box = np.int0(box)

cv.drawContours(img, [box], -1, (0, 255, 0), 5)

print(box)

y1 = int(box[0][1])

x2 = int(box[1][0])

y2 = int(box[1][1])

x3 = int(box[2][0])

roi = img[y2:y1, x2:x3]

# print(y1,x2,y2,x3)

# img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

# blur = cv.GaussianBlur(img_gray, (5, 5), 0)

# ret2, th2 = cv.threshold(blur, 0, 255, cv.THRESH_BINARY + cv.THRESH_OTSU)

# canny = cv.Canny(th2, 100, 200, 3)

# _, contours, hierarchy = cv.findContours(canny, cv.RETR_TREE,

cv.CHAIN_APPROX_NONE)

# img_res = cv.cvtColor(img_gray, cv.COLOR_GRAY2BGR)

# cv.drawContours(img_res, contours, -1, (0, 255, 0), 2)

return roi

def comp(roi):

resizedRoi = cv.resize(roi, (100, 100))

xresizedRoi = cv.inRange(resizedRoi, lower, upper)

# examples resize

noDrive = cv.imread('C:/Users/Denis/Documents/drive/Google Drive/Projects/Computer

Vision/sign/img/noDrive.png',)

noDrive = cv.resize(noDrive, (100, 100))

pedistrain = cv.imread('C:/Users/Denis/Documents/drive/Google Drive/Projects/Computer

Vision/sign/img/pedistrain.png')

pedistrain = cv.resize(pedistrain, (100, 100))

xnoDrive = cv.inRange(noDrive, lower, upper)

xpedistrain = cv.inRange(pedistrain, lower, upper)

cor_noDrive = 0

cor_pedistrain = 0

# попиксельное сравнение

for i in range(100):

for j in range(100):

if xresizedRoi[i][j] == xnoDrive[i][j]:

cor_noDrive += 1

if xresizedRoi[i][j] == xpedistrain[i][j]:

cor_pedistrain += 1

print('cor_noDrive: ' + str(cor_noDrive))

print('cor_pedistrain: ' + str(cor_pedistrain))

if cor_noDrive > 0:

if cor_noDrive > cor_pedistrain:

print('No_drive')

elif cor_noDrive < cor_pedistrain:

print('Pedistrain')

else:

print('Not found')

«hsvSettings.py»(Дополнительное задание)

import cv2 as cv

import numpy as np

cap = cv.VideoCapture(0)

def nothing(x):

pass

# Creating a window for later use

cv.namedWindow('result')

# Starting with 100's to prevent error while masking

h,s,v = 100,100,100

mh,ms,mv = 100,100,100

# Creating track bar

cv.createTrackbar('h', 'result',0,255,nothing)

cv.createTrackbar('s', 'result',0,255,nothing)

cv.createTrackbar('v', 'result',0,255,nothing)

cv.createTrackbar('mh', 'result',0,255,nothing)

cv.createTrackbar('ms', 'result',0,255,nothing)

cv.createTrackbar('mv', 'result',0,255,nothing)

while(1):

_, frame = cap.read()

#frame = cv.imread('C:/Users/Denis/Documents/drive/Google Drive/Projects/Computer

Vision/sign/img/ph1.jpg',

# cv.IMREAD_COLOR)

#converting to HSV

hsv = cv.cvtColor(frame,cv.COLOR_BGR2HSV)

# get info from track bar and appy to result

h = cv.getTrackbarPos('h','result')

s = cv.getTrackbarPos('s','result')

v = cv.getTrackbarPos('v','result')

mh = cv.getTrackbarPos('mh','result')

ms = cv.getTrackbarPos('ms','result')

mv = cv.getTrackbarPos('mv','result')

# Normal masking algorithm

lower_blue = np.array([h,s,v])

upper_blue = np.array([mh,ms,mv])

mask = cv.inRange(hsv,lower_blue, upper_blue)

result = cv.bitwise_and(frame,frame,mask = mask)

cv.imshow('result',result)

k = cv.waitKey(5) & 0xFF

if k == 27:

break

cap.release()

cv.destroyAllWindows()

«hsvSettingsPi.py» (Дополнительное задание)

import cv2 as cv

import os

import numpy as np

import time

from picamera import PiCamera

from picamera.array import PiRGBArray

from defs import pi_imread

def nothing(x):

pass

# Creating a window for later use

cv.namedWindow('result')

# Starting with 100's to prevent error while masking

h,s,v = 100,100,100

mh,ms,mv = 100,100,100

# Creating track bar

cv.createTrackbar('h', 'result',0,255,nothing)

cv.createTrackbar('s', 'result',0,255,nothing)

cv.createTrackbar('v', 'result',0,255,nothing)

cv.createTrackbar('mh', 'result',0,255,nothing)

cv.createTrackbar('ms', 'result',0,255,nothing)

cv.createTrackbar('mv', 'result',0,255,nothing)

while(1):

#_, frame = cap.read()

frame = pi_imread()

#converting to HSV

hsv = cv.cvtColor(frame,cv.COLOR_BGR2HSV)

# get info from track bar and appy to result

h = cv.getTrackbarPos('h','result')

s = cv.getTrackbarPos('s','result')

v = cv.getTrackbarPos('v','result')

mh = cv.getTrackbarPos('mh','result')

ms = cv.getTrackbarPos('ms','result')

mv = cv.getTrackbarPos('mv','result')

# Normal masking algorithm

lower_blue = np.array([h,s,v])

upper_blue = np.array([mh,ms,mv])

mask = cv.inRange(hsv,lower_blue, upper_blue)

result = cv.bitwise_and(frame,frame,mask = mask)

cv.imshow('result',result)

k = cv.waitKey(5) & 0xFF

if k == 27:

break


«instream.py» (Дополнительное задание)

# -*- coding: utf-8 -*-


import numpy as np

import cv2 as cv



lower = np.array([0, 99, 109])

#lower = np.array([0, 61, 204])

upper = np.array([255, 255, 255])

box = []

def img_tr(img):








c = []



#print(c)

#print(c)

if len(c) > 0:



# print(rect)


box = np.int0(box)

#print(box[0])

y1 = int(box[0][1])

x2 = int(box[1][0])

y2 = int(box[1][1])

x3 = int(box[2][0])


cv.drawContours(frame, [box], -1, (0, 255, 0), 2)

#cv.drawContours(img, , -1, (0, 255, 0), 2)

#roi = four_point_transform(img, pts)

#roi = img

#roi = img

else:

print('Not found')

return 0





# canny = cv.Canny(th2, 100, 200, 3)





return roi

def comp(roi):



# examples resize

noDrive = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer

Vision/NTI/img/sign/noDrive.png',)


pedistrain = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer

Vision/NTI/img/sign/pedistrain.png')




cor_noDrive = 0

cor_pedistrain = 0





cor_noDrive += 1


cor_pedistrain += 1

print('cor_noDrive: ' + str(cor_noDrive))

print('cor_pedestrian: ' + str(cor_pedistrain))

if cor_noDrive > 0:


print('Sign found: No_drive')


print('Sign found: Pedestrian')

else:

print('Not found')

else:

print('Not found')

while(True):

ret, frame = cap.read()

gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)

cv.imshow('Video', frame)

# cv2.imshow('frame',gray)

roi = img_tr(frame)

if type(roi) != int:

if roi.any():

try:

comp(roi)

except:

print('Не найдено')

if cv.waitKey(1) & 0xFF == ord('q'):

break

h, w, ch = frame.shape

#print('IMG: w = ' + str(w) + ' h = ' + str(h))

cv.namedWindow('img', cv.WINDOW_NORMAL)

cv.resizeWindow('img', w, h)

cv.imshow('img', frame)

cap.release()


«ped_detector.py»(Дополнительное задание)

# -*- coding: utf-8 -*-

# детектор пешехода

import cv2 as cv

path = 'C:/Users/Quantum/Desktop/NTI_ATS/Computer Vision/NTI/img/pedistrain/'

filename = 'im1.jpg'

def ped_detect(img):

#print(img.shape[1])

k = img.shape[1]/320.0

img = cv.resize(img, (int(img.shape[1]/k), int(img.shape[0]/k)))

#img = cv.resize(img, (int(180), int(320)))

rects, weights = hog.detectMultiScale(img)

#print('Contours found: ' + str(len(rects)))

max_area = 0

x_m, y_m, w_m, h_m = 0, 0, 0, 0

for x, y, w, h in rects:

pad_w, pad_h = int(0.05*w), int(0.05*h)

if (w-x)*(h-y) > max_area:

max_area = (w-x)*(h-y)

x_m, y_m, w_m, h_m = x, y, w, h

if max_area != 0:

print('KOZHANY UBLUDOK DETECTED')

pad_w, pad_h = int(0.1 * w_m), int(0.1 * h_m)

font = cv.FONT_HERSHEY_SIMPLEX

cv.putText(img, 'KOZHANY UBLUDOK', (x+10, y+10+h_m), font, 0.6, (0, 255, 0), 1,

cv.LINE_AA)

cv.rectangle(img, (x_m + pad_w, y_m + pad_h), (x_m + w_m - pad_w, y_m + h_m -

pad_h), (0, 255, 0), 2)

else:

print('НЕ НАЙДЕНО')

return(img)

hog = cv.HOGDescriptor()

hog.setSVMDetector(cv.HOGDescriptor_getDefaultPeopleDetector())

img = cv.imread(path+filename)

img_res = ped_detect(img)

cv.imshow('RESULT', img_res)

cv.waitKey(0)

«sign_detector.py» (Дополнительное задание)

# -*- coding: utf-8 -*-


import cv2 as cv

import os

import numpy as np

import time

print (cv.__file__)

import math

# from matplotlib import pyplot as plt



lower = np.array([0, 81, 145])

upper = np.array([255, 255, 255])

def imread():

#img = cv.imread('C:/Users/Quantum/Desktop/Computer Vision/sign/img/sign/ex_1.jpg',

# cv.IMREAD_COLOR)

ret_val, img = cap.read()

#cv.imshow('Preview', img)

#_, frame = cap.read()

return img

def img_tr(img):








c = []



#print(c)

#print(c)

if len(c) > 0:



# print(rect)


box = np.int0(box)

#print(box[0])

y1 = int(box[0][1])

x2 = int(box[1][0])

y2 = int(box[1][1])

x3 = int(box[2][0])


cv.drawContours(img, [box], -1, (0, 255, 0), 2)

#cv.drawContours(img, , -1, (0, 255, 0), 2)

#roi = four_point_transform(img, pts)

#roi = img

#roi = img

else:

print('Not found')

return 0





# canny = cv.Canny(th2, 100, 200, 3)





return roi

def comp(roi):



# examples resize

noDrive = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer

Vision/NTI/img/sign/noDrive.png',)


pedistrain = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer

Vision/NTI/img/sign/pedistrain.png')




cor_noDrive = 0

cor_pedistrain = 0





cor_noDrive += 1


cor_pedistrain += 1

#print('cor_noDrive: ' + str(cor_noDrive))

#print('cor_pedistrain: ' + str(cor_pedistrain))

if cor_noDrive > 0:


print('No_drive')


print('Pedistrain')

else:

print('Not found')

else:

print('Not found')

while True:

img = imread()

#img = cv.imread('C:/Users/Quantum/Desktop/Computer

Vision/NTI/img/sign/ph1.jpg',cv.IMREAD_COLOR)

roi = img_tr(img)

try:

if roi.any():

comp(roi)

h, w, ch = img.shape

#print('IMG: w = ' + str(w) + ' h = ' + str(h))

cv.namedWindow('img', cv.WINDOW_NORMAL)

cv.resizeWindow('img', w, h)

cv.imshow('img', img)

except:

print('not found')

Аэронет


Код программы на языке С++, обеспечивающий решение задач подтрека:

«IR.ino»




IRdecode myDecoder;



byte k;

void setupIR() {

delay(500);


}

void loopIR() {

unsigned long int TIMESV = millis();

bool valueIR = 0;

delay(2000);

waitLED();

while (valueIR == 0) {

// if (TIMESV + 500 < millis()) {

// k++;

// waitLED();///////////////////////////////////проверить скорость переключения

// TIMESV = millis();

// if (k == 2) k = 0;

// }


{


if (myDecoder.protocolNum == SONY) { }



zahvatLED();

zahvatMagnit(1);

valueIR = 1;

}

}

nh.spinOnce();

}

delay(500);

}

«NTI_Task2.ino»

/////////////////////////////////////////

// Размер системы координат полигона //

// max x = 153 см //

// max y = 270 см //

// maz z = 140 см //

/////////////////////////////////////////

///////////////setting-ros///////////////

// Калининград тащит

// топ код с поиском нужной точки для сброса груза

// у других команд этого нет

// но вы не читайте.

// мы исправили то недарузумение прошлого года

// когда калининградцы жестко слили (Tau)

#include <ros.h>

#include <clever/GetTelemetry.h>

#include <clever/Navigate.h>

#include <mavros_msgs/SetMode.h>

typedef ros::NodeHandle_<ArduinoHardware, 3, 3, 100, 100> NodeHandle;

using namespace clever;

using namespace mavros_msgs;

NodeHandle nh;

ros::ServiceClient<Navigate::Request, Navigate::Response> navigate("/navigate");

ros::ServiceClient<SetMode::Request, SetMode::Response> setMode("/mavros/set_mode");

ros::ServiceClient<GetTelemetry::Request, GetTelemetry::Response>

getTelemetry("/get_telemetry");

/////////////////////////////////////////

unsigned long int lastTime;

#define posadkaLEDdelay 5000

void setup() {

setupIR();

setupMagnit();

setupLED();

// Инициализация rosserial

nh.initNode();

// Инициализация сервисов

nh.serviceClient(navigate);

nh.serviceClient(setMode);

nh.serviceClient(getTelemetry);

// Ожидание подключение к Raspberry Pi

while (!nh.connected()) nh.spinOnce();

nh.loginfo("Startup complete");

// Пользовательская настройка

// <...>

// Тестовая программа

Navigate::Request nav_req;

Navigate::Response nav_res;

SetMode::Request sm_req;

SetMode::Response sm_res;

GetTelemetry::Request gt_req;

GetTelemetry::Response gt_res;

// старт

loopIR();

// взлет // относ себя

nh.loginfo("Take off");

nav_req.auto_arm = true;

nav_req.x = 0;

nav_req.y = 0;

nav_req.z = 0.7;

nav_req.frame_id = "fcu_horiz";

nav_req.speed = 0.5;

navigate.call(nav_req, nav_res);

vzletLED();

delaySTOP(3000);

zavisanieLED();

delaySTOP(4000);

nh.loginfo("Fly on point"); // полет в Б // относ карты

nav_req.x = 0.55;

nav_req.y = 0.55;

nav_req.z = 0.80; // высота от пола - 70 см

nav_req.frame_id = "aruco_map";

nav_req.update_frame = true;


nav_req.yaw = 1.57;


pereletLED();

delaySTOP(4000);

zavisanieLED();

delaySTOP(1000);

nh.loginfo("Fly on point"); // снижение в Б // относ карты

nav_req.x = 0.57;

nav_req.y = 0.54;





nav_req.yaw = 1.57;


pereletLED();

delaySTOP(2000);

zavisanieLED();

delaySTOP(1000);

nh.loginfo("Fly on point"); // снижение 2 в Б // относ карты

nav_req.x = 0.58;

nav_req.y = 0.53;





nav_req.yaw = 1.57;


pereletLED();

delaySTOP(1000);

gt_req.frame_id = "aruco_map"; // фрейм для значений x, y, z

getTelemetry.call(gt_req, gt_res);

while (!((gt_res.x >= 0.55 - 0.02) and (gt_res.x <= 0.55 + 0.02) and (gt_res.y >= 0.55 - 0.02)

and (gt_res.y <= 0.55 + 0.02))) {



nh.spinOnce();

}

zahvatMagnit(0);

NOzahvatLED();

// сброс груза

delaySTOP(2000);

nh.loginfo("Fly on point"); // поднятие в Б // относ карты

nav_req.x = 0.57;

nav_req.y = 0.55;





nav_req.yaw = 1.57;


pereletLED();

delaySTOP(2000);

zavisanieLED();

delaySTOP(1000);

nh.loginfo("Fly on point"); // полет в В

nav_req.x = 0.90;

nav_req.y = 1.51;

nav_req.z = 0.9; //высота от пола - 50 см


nav_req.update_frame = false;


nav_req.yaw = 1.57;


pereletLED();

delaySTOP(4000);

zavisanieLED();

delaySTOP(2000);

nh.loginfo("Fly on point"); // преземление в В

nav_req.x = 0.90;

nav_req.y = 1.51;





nav_req.yaw = 1.57;


pereletLED();

delaySTOP(2000);

nh.loginfo("Fly on point"); // преземление 2 в В

nav_req.x = 0.90;

nav_req.y = 1.51;





nav_req.yaw = 1.57;


pereletLED();

delaySTOP(2000);



while (!((gt_res.x >= 0.90 - 0.02) and (gt_res.x <= 0.90 + 0.02) and (gt_res.y >= 1.51 - 0.02)

and (gt_res.y <= 1.51 + 0.02))) {



nh.spinOnce();

}

nh.loginfo("Fly on point"); // преземление 2 в В

nav_req.x = 0.90;

nav_req.y = 1.51;





nav_req.yaw = 1.57;


pereletLED();

// Посадка

nh.loginfo("Land");

sm_req.custom_mode = "AUTO.LAND";

setMode.call(sm_req, sm_res);

posadkaLED();

}

void loop() {

}

void delaySTOP(int vbn) {


while (lastTime + vbn > millis()) {

delay(100);

nh.spinOnce();

}

}

«Zahvat.ino»

void setupMagnit() {

pinMode(5, OUTPUT);

}

void zahvatMagnit(bool i){

digitalWrite(5, i);

}

«led.ino»

//////////////////////////////////////

// клешня разжата - red //

// клешня сжата - green //

// //

// перелет - purple //

// взлет - r-g-b //

// зависание - white //

// преземление - сюрприз //

//////////////////////////////////////

#include "Adafruit_NeoPixel.h"

#define LED_COUNT 30

#define LED_PIN 10

Adafruit_NeoPixel strip = Adafruit_NeoPixel(LED_COUNT, LED_PIN, NEO_GRB +

NEO_KHZ800);

void setupLED() {

strip.begin();

NOzahvatLED();

}

void loopLED()

{

}

void zahvatLED() { // клешня сжата - green


{


}

strip.show();

}

void NOzahvatLED() { // клешня разжата - red


{


}

strip.show();

}

void zavisanieLED() { // зависание - white


{


}

strip.show();

}

void pereletLED() { // перелет - purple


{


}

strip.show();

}

void vzletLED() { // взлет - r-g-b

static byte c[5] = {255, 0, 0, 255, 0};


byte j = i % 3;

strip.setPixelColor(i, strip.Color(c[j], c[j + 1], c[j + 2]));

}

strip.show();

}

void posadkaLED() { // посадка - переливание


while (lastTime + 5000 > millis() ) {

for (byte k = 0; k < 3; k++) {

static byte c[7] = {255, 0, 0, 255, 0, 0, 255};

for (byte i = 0; i < LED_COUNT - 1; i++) {

int j = i % 3;


}

strip.show();

}

}

}

void waitLED() { // ожидание - переливание

static int c[7] = {200, 200, 0, 200, 200, 0, 200};


int j = i % 3;


strip.show();

}

}

Зайцев Егор Владимировичnti-contest.ru › wp-content › uploads › res-2018...

Documents

Transcript of Зайцев Егор Владимировичnti-contest.ru › wp-content › uploads › res-2018...