%d0%a1%d0%b1%d0%be%d1%80%d0%bd%d0%b8%d0%ba %d0%a1%d1%8a%d0%b5%d0%b7%d0%b4%d0%a2%d0%b5%d1%80%d0%b0%d0
Зайцев Егор Владимировичnti-contest.ru › wp-content › uploads › res-2018...
Transcript of Зайцев Егор Владимирович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.
Информатика
Задача 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 - Команда с тремя устройствами в сборе
Код программы на языке С++, обеспечивающий решение задач подтрека:
«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);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, 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));
digitalWrite(IN1, 1);
}
if (rgt < 0) {
analogWrite(IN3, 255 - abs(rgt));
digitalWrite(IN4, 1);
} else {
analogWrite(IN3, rgt);
digitalWrite(IN4, 0);
}
}
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';
}
if (myDecoder.value == 0x50) {
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(cord[1]);
Serial.println(cord[2]);
Serial.println("\n");
str = "";
}
}
}
Автонет
Фото устройства:
Код программы на языках С++/Python, обеспечивающий решение задач подтрека:
«check.ino»
// получает координату
#include <Servo.h>
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#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);
Serial3.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);
// }
if (lastTime + 50 < millis()) {
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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = false;
Serial.println("Dir changed");
}
void forward() {
if (flag) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = true;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = true;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
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 Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#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);
Serial3.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);
// }
if (lastTime + 50 < millis()) {
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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = false;
Serial.println("Dir changed");
}
void forward() {
if (flag) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = true;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = true;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
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);
}
«IR_example.ino»
//https://github.com/cyborg5/IRLib2
#include <IRLibDecodeBase.h>
#include <IRLib_P02_Sony.h>
#include <IRLibCombo.h>
IRdecode myDecoder;
#include <IRLibRecv.h>
IRrecv myReceiver(11); //Пин куда подключен сигнал приемника
void setupIR() {
myReceiver.enableIRIn(); // Запуск ресивера
Serial.println(("Ready to receive IR signals"));
}
char getIR() {
if (myReceiver.getResults())
{
myDecoder.decode(); // Декодируем...
if (myDecoder.protocolNum == SONY) {
Serial.println(myDecoder.value);
}
myReceiver.enableIRIn(); // Перезапускаем приемник
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>
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 90 // вправо
#define dirF 112 // прямо
#define dirL 130 // влево
int flag = 1; //переключение мотора(вперед)
char c = 'F';
int start = 1;
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
STOP();
delay(1000);
}
void loop() {
if(start == 1) {
STOP();
delay(700);
start = 0;
}
//if (Serial.available()) c = Serial.parseInt();
c = 'F';
DIRECTION(c);
forward();
delay(2000);
back();
delay(2000);
}
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(1350);
delay(100);
myservo.writeMicroseconds(1500);
delay(100);
myservo.writeMicroseconds(1350);
delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(1350);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(1590);
Serial.println("back");
flag = 1;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
«eight_move.ino»
//2с вперед, 2с назад
#include <Servo.h>
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 90 // вправо
#define dirF 112 // прямо
#define dirL 130 // влево
int flag = 1; //переключение мотора(вперед)
char c = 'F';
int start = 1;
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
STOP();
delay(1000);
}
void loop() {
if(start == 1) {
STOP();
delay(700);
start = 0;
}
//if (Serial.available()) c = Serial.parseInt();
c = 'R';
DIRECTION(c);
forward();
delay(2000);
c = 'L';
DIRECTION(c);
delay(2000);
}
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(1350);
delay(100);
myservo.writeMicroseconds(1500);
delay(100);
myservo.writeMicroseconds(1350);
delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(1350);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(1590);
Serial.println("back");
flag = 1;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("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
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(255, 0, 0));
}
strip.show();
}
void zavisanieLED() { // зависание - white
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(127, 127, 127));
}
strip.show();
}
void pereletLED() { // перелет - purple
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(255, 0, 255));
}
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};
for (int 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();
}
}»
// получает координату
#include <Servo.h>
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#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
double distC = 0;
double distL = 0;
double distR = 0;
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;
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
Serial3.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);
Serial.println(x);
GetCoordinate();
//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);
// }
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 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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = false;
Serial.println("Dir changed");
}
void forward() {
if (flag) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = true;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = true;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
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);
}
«prilipala.ino»
// следует за препятствием в 30см
#include <Servo.h>
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальнчомер
#define MAX_DISTANCE 200
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 70 // вправо
#define dirF 94 // прямо
#define dirL 110 // влево
#define SpeedF 1400
#define SpeedB 1590
double distC = 0;
double distL = 0;
double distR = 0;
int stopped = 1;
int flag = 1; //переключение мотора(вперед)
char c = 'F';
int start = 1;
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
STOP();
delay(500);
}
void loop() {
if (start == 1) {
STOP();
delay(700);
start = 0;
back();
delay(500);
forward();
delay(100);
}
distC = getSonar(Csonar);
//distR = getSonar(Rsonar);
//distL = getSonar(Lsonar);
//if (Serial.available()) c = Serial.parseInt();
c = 'F';
DIRECTION(c);
Serial.println(distC);
if (distC < 1 && stopped) forward();
else if (distC < 20 && stopped) fastSTOP();
}
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(50);
myservo.writeMicroseconds(1500);
delay(50);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
//delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = 1;
//delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
void fastSTOP() {
if (stopped == 1) {
myservo.writeMicroseconds(1800);
delay(50);
stopped = 0;
}
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
return pulseIn(SonarPin, HIGH, 15000) / 58;
}
«with_get_coord.ino»
// получает координату
#include <Servo.h>
#include <IRLibDecodeBase.h>
#include <IRLib_P02_Sony.h>
#include <IRLibCombo.h>
IRdecode myDecoder;
#include <IRLibRecv.h>
IRrecv myReceiver(10); //Пин куда подключен сигнал приемника
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 70 // вправо
#define dirF 94 // прямо
#define dirL 110 // влево
#define SpeedF 1400
#define SpeedB 1590
double distC = 0;
double distL = 0;
double distR = 0;
int stopped = 1; // флаг для быстрой остановки
int flag = 1; //переключение мотора(вперед)
char c = 'F'; // символ для управления направлением сервы
int start = 1; // подключение движков, флаг
int x = 5; // координаты и угол
int y = 5;
int angle = -90;
char stopSignal = 'L'; // сигнал для остановки
char starSignal = 'R'; // сигнал для езды
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
Serial3.begin(9600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
setupIR();
STOP();
delay(500);
}
void loop() {
if (start == 1) {
STOP();
delay(700);
start = 0;
back();
delay(500);
forward();
delay(100);
}
//distC = getSonar(Csonar);
//distR = getSonar(Rsonar);
//distL = getSonar(Lsonar);
GetCoordinate();
//if (Serial.available()) c = Serial.parseInt();
c = 'F';
DIRECTION(c);
// Остановка по препятствию
//Serial.println(distC);
//if (distC < 20) fastSTOP();
// Работа с ИК
// if (getIR() == stopSignal) {
// STOP();
// }
// else if (getIR() == startSignal) {
// forward();
// }
forward();
delay(100);
}
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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = 1;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
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);
//str = "00/123/456/-179"; // пример строки
if (str.substring(0, 2).toInt() == -5)
{ Serial.println("Marker not found");
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=");
Serial.print(y);
Serial.print(" :: Angle=");
Serial.println(angle);
}
«with_get_IR.ino»
// следует за препятствием в 30см
#include <Servo.h>
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 70 // вправо
#define dirF 94 // прямо
#define dirL 110 // влево
#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;
int flag = 1; //переключение мотора(вперед)
char c = 'F';
int start = 1;
char stopSignal = 'L';
char startSignal = 'R';
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(57600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
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);
//if (Serial.available()) c = Serial.parseInt();
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 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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(SpeedF);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = 1;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
void fastSTOP() {
if (stopped == 1) {
myservo.writeMicroseconds(1800);
delay(50);
//stopped = 0;
}
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
return pulseIn(SonarPin, HIGH, 15000) / 58;
}
«zadanie_4.ino»
//движение по светофору
#include <Servo.h>
#define Csonar 12 // Центральный дальномер
#define MAX_DISTANCE 200
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 70 // вправо
#define dirF 90 // прямо
#define dirL 110 // влево
double distC;
int flag = 1; //переключение мотора(вперед)
char c = 'F';
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
STOP();
delay(500);
}
void loop() {
//if (Serial.available()) c = Serial.parseInt();
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 DIRECTION(char c) {
if (c == 'L') myservo2.write(dirL);
else if (c == 'R') myservo2.write(dirR);
else myservo2.write(dirF);
}
void changeDir() {
myservo.writeMicroseconds(1380);
delay(100);
myservo.writeMicroseconds(1500);
delay(100);
myservo.writeMicroseconds(1380);
//delay(100);
flag = 0;
Serial.println("Dir changed");
}
void forward() {
if (flag == 1) changeDir();
myservo.writeMicroseconds(1380);
Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(1570);
Serial.println("back");
flag = 1;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = 1;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
return pulseIn(SonarPin, HIGH, 15000) / 58;
}
«motor.ino»
#include <Servo.h>
#include <NewPing.h>
#define TRIGGER_PIN 15
#define ECHO_PIN 16
#define MAX_DISTANCE 200
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);
Serial.println("STOP");
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);
Serial.println("back");
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;
byte dir = 100; // 100 - прямо, 80 - вправо, 110 - влево
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);
myservo.attach(M_PIN);
myservo2.attach(D_PIN);
myservo.writeMicroseconds(1500);
delay(600);
}
void loop() {
// put your main code here, to run repeatedly:
if(Serial.available()){
sp = Serial.parseInt();
}
forward(sp);
delay(10);
}
void STOP(){
myservo.writeMicroseconds(1500);
Serial.println("STOP");
delay(50);
}
void forward(int sp){
//byte sp1;
//sp = map(sp1, 0, 100, 104, 140);
myservo.writeMicroseconds(sp);
//myservo.writeMicroseconds(sp);
Serial.println("forward");
Serial.print(sp);
delay(10);
}
void changeDir(){
myservo.writeMicroseconds(1400);
delay(100);
myservo.writeMicroseconds(1500);
delay(100);
myservo.writeMicroseconds(1400);
//delay(100);
Serial.println("Dir changed");
}
void back(){
myservo.writeMicroseconds(1570);
Serial.println("back");
delay(10);
}
void start(){
myservo.writeMicroseconds(1500);
Serial.println("start");
}
«motor_test.ino»
#include <Servo.h>
#include <NewPing.h>
#define TRIGGER_PIN 15
#define ECHO_PIN 16
#define MAX_DISTANCE 200
#define M_PIN 9
void STOP();
void forward(int);
void back();
int Speed = 50;
byte dir = 90; // 90 - прямо, 60 - вправо, 120 - влево
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(){
myservo.writeMicroseconds(1500);
Serial.println("STOP");
delay(50);
}
void forward(int sp){
//byte sp1;
//sp = map(sp1, 0, 100, 104, 140);
myservo.writeMicroseconds(sp);
Serial.println("forward"+String(sp));
delay(10);
}
void back(){
myservo.writeMicroseconds(70);
Serial.println("back");
delay(10);
}
«copy.ino»
// получает координату
#include <Servo.h>
#define Lsonar 34 // Левый дальномер
#define Rsonar 36 // Правый дальномер
#define Csonar 32 // Центральный дальномер
#define MAX_DISTANCE 200
#define D_PIN 8 // сервопривод поворота
#define M_PIN 9 // драйвер моторов
#define dirR 75// вправо
#define dirF 95 // прямо
#define dirL 120 // влево
#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)
int angleServoint = 95;
float angleServofl = 95.0;
double distC = 0;
double distL = 0;
double distR = 0;
bool proverka = false;
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; // последний сохраненный
char stopSignal = 'L';
char startSignal = 'R';
int x = 5; // координаты и угол
int y = 5;
int angle = -90;
Servo myservo;
Servo myservo2;
void setup() {
Serial.begin(9600);
Serial3.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);
STOP();
GetCoordinate();
// 1 участок, по прямой
delay(500);
GetCoordinate();
delay(500);
GetCoordinate();
Serial.println("ANGLE");
Serial.println(angle);
start = 0;
}
proverka = compIR();
if (proverka){
if (angle > -87) {
//angleServofl = angleServoint - angle * 0.05;
myservo2.write(dirF - 10);
Serial.println("left");
}
else if (angle < -90) {
//angleServofl = angleServoint - angle * 0.05;
myservo2.write(dirF + 20);
Serial.println("right");
}
else myservo2.write(dirF);
delay(300);
forward();
delay(100);
while (x < x_st1) {
GetCoordinate();
if (angle > -87) {
//angleServofl = angleServoint - angle * 0.05;
myservo2.write(dirF - 10);
Serial.println("left");
}
else if (angle < -90) {
//angleServofl = angleServoint - angle * 0.05;
myservo2.write(dirF + 12);
Serial.println("right");
}
else myservo2.write(dirF);
}
//2 участок - окружность
GetCoordinate();
myservo2.write(dirL);
while (y < y_st2) {
GetCoordinate();
forward();
}
//myservo2.write(dirF-10);
// 3 участок - выпрямление
myservo2.write(dirF);
//int angleB = atan2(abs(x - x_st3), abs(y - y_st3));
while (y < y_st4) {
forward();
GetCoordinate();
myservo2.write(dirF - 10);
}
// 4 участок - поворот к порту
while (x > x_end-10) {
GetCoordinate();
myservo2.write(dirL);
}
Serial.println("STOP");
myservo2.write(dirF);
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");
// }
// }
lastTime = millis();
}
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);
myservo.writeMicroseconds(SpeedF);
//delay(100);
flag = false;
Serial.println("Dir changed");
}
void forward() {
if (flag) changeDir();
myservo.writeMicroseconds(SpeedF);
//Serial.println("forward");
delay(10);
}
void back() {
myservo.writeMicroseconds(SpeedB);
Serial.println("back");
flag = true;
delay(10);
}
void STOP() {
myservo.writeMicroseconds(1500);
Serial.println("STOP");
flag = true;
}
int getSonar(int SonarPin) {
pinMode(SonarPin, OUTPUT);
digitalWrite(SonarPin, HIGH);
delayMicroseconds(10);
digitalWrite(SonarPin, LOW);
pinMode(SonarPin, INPUT);
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');
//String str = Serial3.readString();
// Serial.print("str : ");
//Serial.println("HERE");
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();
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);
}
«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
cv.destroyAllWindows()
«instream.py» (Дополнительное задание)
# -*- coding: utf-8 -*-
# распознавание дорожных знаков
import numpy as np
import cv2 as cv
cap = cv.VideoCapture(0)
# пороговые значения, из hsvSettings
lower = np.array([0, 99, 109])
#lower = np.array([0, 61, 204])
upper = np.array([255, 255, 255])
box = []
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) # поиск контуров
c = []
for cnt in contours:
c = sorted(contours, key=cv.contourArea, reverse=True)[0] # сортировка по площади
#print(c)
#print(c)
if len(c) > 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)
#print(box[0])
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]
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
# 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/Quantum/Desktop/NTI_ATS/Computer
Vision/NTI/img/sign/noDrive.png',)
noDrive = cv.resize(noDrive, (100, 100))
pedistrain = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer
Vision/NTI/img/sign/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_pedestrian: ' + str(cor_pedistrain))
if cor_noDrive > 0:
if cor_noDrive > cor_pedistrain:
print('Sign found: No_drive')
elif cor_noDrive < cor_pedistrain:
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()
cv.destroyAllWindows()
«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
cap = cv.VideoCapture(0)
# пороговые значения, из hsvSettings
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):
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) # поиск контуров
c = []
for cnt in contours:
c = sorted(contours, key=cv.contourArea, reverse=True)[0] # сортировка по площади
#print(c)
#print(c)
if len(c) > 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)
#print(box[0])
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]
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
# 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/Quantum/Desktop/NTI_ATS/Computer
Vision/NTI/img/sign/noDrive.png',)
noDrive = cv.resize(noDrive, (100, 100))
pedistrain = cv.imread('C:/Users/Quantum/Desktop/NTI_ATS/Computer
Vision/NTI/img/sign/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')
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»
#include <IRLibDecodeBase.h>
#include <IRLib_P02_Sony.h>
#include <IRLibCombo.h>
IRdecode myDecoder;
#include <IRLibRecv.h>
IRrecv myReceiver(9); //Пин куда подключен сигнал приемника
byte k;
void setupIR() {
delay(500);
myReceiver.enableIRIn(); // Запуск ресивера
}
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 (myReceiver.getResults())
{
myDecoder.decode(); // Декодируем...
if (myDecoder.protocolNum == SONY) { }
myReceiver.enableIRIn(); // Перезапускаем приемник
if (myDecoder.value == 0x10) {
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.speed = 0.5;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
delaySTOP(4000);
zavisanieLED();
delaySTOP(1000);
nh.loginfo("Fly on point"); // снижение в Б // относ карты
nav_req.x = 0.57;
nav_req.y = 0.54;
nav_req.z = 1.33; // высота от пола - 7 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = true;
nav_req.speed = 0.4;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
delaySTOP(2000);
zavisanieLED();
delaySTOP(1000);
nh.loginfo("Fly on point"); // снижение 2 в Б // относ карты
nav_req.x = 0.58;
nav_req.y = 0.53;
nav_req.z = 1.51; // высота от пола - 0 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = true;
nav_req.speed = 0.4;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
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))) {
gt_req.frame_id = "aruco_map"; // фрейм для значений x, y, z
getTelemetry.call(gt_req, gt_res);
nh.spinOnce();
}
zahvatMagnit(0);
NOzahvatLED();
// сброс груза
delaySTOP(2000);
nh.loginfo("Fly on point"); // поднятие в Б // относ карты
nav_req.x = 0.57;
nav_req.y = 0.55;
nav_req.z = 0.9; // высота от пола - 7 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = true;
nav_req.speed = 0.4;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
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.frame_id = "aruco_map";
nav_req.update_frame = false;
nav_req.speed = 0.5;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
delaySTOP(4000);
zavisanieLED();
delaySTOP(2000);
nh.loginfo("Fly on point"); // преземление в В
nav_req.x = 0.90;
nav_req.y = 1.51;
nav_req.z = 1.1; //высота от пола - 30 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = false;
nav_req.speed = 0.5;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
delaySTOP(2000);
nh.loginfo("Fly on point"); // преземление 2 в В
nav_req.x = 0.90;
nav_req.y = 1.51;
nav_req.z = 1.47; //высота от пола - 30 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = false;
nav_req.speed = 0.5;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
delaySTOP(2000);
gt_req.frame_id = "aruco_map"; // фрейм для значений x, y, z
getTelemetry.call(gt_req, gt_res);
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))) {
gt_req.frame_id = "aruco_map"; // фрейм для значений x, y, z
getTelemetry.call(gt_req, gt_res);
nh.spinOnce();
}
nh.loginfo("Fly on point"); // преземление 2 в В
nav_req.x = 0.90;
nav_req.y = 1.51;
nav_req.z = 1.54; //высота от пола - 30 см
nav_req.frame_id = "aruco_map";
nav_req.update_frame = false;
nav_req.speed = 0.5;
nav_req.yaw = 1.57;
navigate.call(nav_req, nav_res);
pereletLED();
// Посадка
nh.loginfo("Land");
sm_req.custom_mode = "AUTO.LAND";
setMode.call(sm_req, sm_res);
posadkaLED();
}
void loop() {
}
void delaySTOP(int vbn) {
lastTime = millis();
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
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(0, 255, 0));
}
strip.show();
}
void NOzahvatLED() { // клешня разжата - red
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(255, 0, 0));
}
strip.show();
}
void zavisanieLED() { // зависание - white
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(127, 127, 127));
}
strip.show();
}
void pereletLED() { // перелет - purple
for (int i = 0; i < LED_COUNT - 1; i++)
{
strip.setPixelColor(i, strip.Color(255, 0, 255));
}
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};
for (int 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();
}
}