#include <Servo.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "servo.h"
#define rightIR 11
#define rearIR 5
#define frontIR 3
#define int1 28 // Направление вращения двигателя 1
#define int2 26 // Направление вращения двигателя 1
#define int3 24 // Направление вращения двигателя 2
#define int4 22 // Направление вращения двигателя 2
#define sp1 8
#define sp2 7
#define KursPin 10
#define TungPin 9
#define ForwardButton 'F' //0x5F12E8C4 //up button
#define BackButton 'B' //0x189D7928 //down button
#define RightButton 'R' //0x68733A46 //right button
#define LeftButton 'L' //0x83B19366 //left button
#define LeftSpeedUpButton 'D' //plus button
#define LeftSpeedDownButton 'E' //minus button
#define StopProgramm 'H'
#define StartProgButton 'G'//0x89964248 //start button
#define StopProgButton 'S'//0xE13DDA28 //stop button
#define RightForwardButton 'C'
#define LeftForwardButton 'A'
#define RootPermission 'I'
#define RootExit 'J'
#define RightSpeedUpButton 'K'
#define RightSpeedDownButton 'M'
#define LostConnection 'N'
#define SKursLeft 'T'
#define SKursRight 'O'
#define STungRight 'P'
#define STungLeft 'Q'
#define defaultpos 'U'
#define EchoPin 4
#define TrigPin 2
#define ChangeSpeed 5
#define ChangeAngle 5
int save = 0;
int speed1 = 255;
int speed2 = 255;
int rootflag = 0;
int tungpos = 90;
int kurspos = 90;
int KursMaxAngle = 85;
int TungMaxAngle = 35;
Servo servokurs; //объявляем переменную Servo
Servo servotung;
void forward()
{
analogWrite(sp1, speed1);
analogWrite(sp2, speed2);
Serial.print(millis());
Serial.print("\t");
Serial.print("forward\n");
digitalWrite(int1, LOW);
digitalWrite(int2, HIGH);
digitalWrite(int3, HIGH);
digitalWrite(int4, LOW);
return;
}
void back()
{
analogWrite(sp1, speed1);
analogWrite(sp2, speed2);
Serial.print(millis());
Serial.print("\t");
Serial.print("back\n");
digitalWrite(int1, HIGH);
digitalWrite(int2, LOW);
digitalWrite(int3, LOW);
digitalWrite(int4, HIGH);
return;
}
void right()
{
Serial.print(millis());
Serial.print("\t");
Serial.print("right\n");
analogWrite(sp1, speed1);
analogWrite(sp2, speed2);
digitalWrite(int1, LOW);
digitalWrite(int2, HIGH);
digitalWrite(int3, LOW);
digitalWrite(int4, HIGH);
return;
}
void left()
{
Serial.print(millis());
Serial.print("\t");
Serial.print("left\n");
analogWrite(sp1, speed1);
analogWrite(sp2, speed2);
digitalWrite(int1, HIGH);
digitalWrite(int2, LOW);
digitalWrite(int3, HIGH);
digitalWrite(int4, LOW);
return;
}
void stop1()
{
Serial.print(millis());
Serial.print("\t");
Serial.print("stop\n");
digitalWrite(int1, LOW);
digitalWrite(int2, LOW);
digitalWrite(int3, LOW);
digitalWrite(int4, LOW);
return;
}
void rightforward()
{
analogWrite(sp1, speed1 - 150);
analogWrite(sp2, speed2);
Serial.print(millis());
Serial.print("\t");
Serial.print("rightforward\n");
digitalWrite(int1, LOW);
digitalWrite(int2, HIGH);
digitalWrite(int3, HIGH);
digitalWrite(int4, LOW);
return;
}
void leftforward()
{
analogWrite(sp1, speed1);
analogWrite(sp2, speed1 - 150);
Serial.print(millis());
Serial.print("\t");
Serial.print("leftworward\n");
digitalWrite(int1, LOW);
digitalWrite(int2, HIGH);
digitalWrite(int3, HIGH);
digitalWrite(int4, LOW);
return;
}
void servokursright()
{
Serial.println("kursright");
if ( kurspos < 90 + KursMaxAngle)
{
kurspos -= ChangeAngle;
}
servokurs.write(kurspos);
Serial.println(kurspos);
}
void servokursleft()
{
Serial.println("kursleft");
if ( kurspos > 90 - KursMaxAngle)
{
kurspos += ChangeAngle;
}
servokurs.write(kurspos);
Serial.println(kurspos);
}
void servotungright()
{
Serial.println("tungright");
if ( tungpos < 90 + TungMaxAngle)
{
tungpos += ChangeAngle;
}
servotung.write(tungpos);
Serial.println(tungpos);
}
void servotungleft()
{
Serial.println("tungleft");
if ( tungpos > 90 - TungMaxAngle)
{
tungpos -= ChangeAngle;
}
servotung.write(tungpos);
Serial.println(tungpos);
}
void DefaultPosition ()
{
Serial.println("defpos");
servotung.write(90);
servokurs.write(90);
return;
}
void porog()
{
int fIR;
int rIR;
fIR = digitalRead(frontIR);
rIR = digitalRead(rearIR);
if ( fIR == 1 || rIR == 1)
{
if ( fIR == 1 )
{
Serial.print(millis());
Serial.print("\t");
Serial.print("danger in front use left right or root\n");
back();
delay(300);
stop1();
}
if ( rIR == 1 )
{
Serial.print(millis());
Serial.print("\t");
Serial.print("danger on back use left right or root\n");
forward();
delay(300);
stop1();
}
//save = 1;
//Serial.print("Use Left, Right, Back or Root to continue... \n");
//UDR0==0x00;
while (1)
{
if (Serial.available())
{
int res;
res = Serial.read();
if (res == RootPermission)
{
root();
return;
}
switch (res)
{
case RightButton:
right();
return;
break;
case LeftButton:
left();
return;
break;
case SKursRight:
servokursright();
break;
case SKursLeft:
servokursleft();
break;
case STungRight:
servotungright();
break;
case STungLeft:
servotungleft();
break;
case defaultpos:
DefaultPosition ();
break;
}
}
}
}
}
void KeySelect (int res)
{
switch (res)
{
case ForwardButton:
forward();
break;
case BackButton:
back();
break;
case RightButton:
right();
break;
case LeftButton:
left();
break;
case StopProgButton:
stop1();
break;
case RightForwardButton:
rightforward();
break;
case LeftForwardButton:
leftforward();
break;
case LostConnection:
connection();
break;
case SKursRight:
servokursright();
break;
case SKursLeft:
servokursleft();
break;
case STungRight:
servotungright();
break;
case STungLeft:
servotungleft();
break;
case defaultpos:
DefaultPosition ();
break;
case LeftSpeedUpButton:
if (speed1 < 254)
{
speed1 += ChangeSpeed;
Serial.print(millis());
Serial.print("\t");
Serial.println(speed1);
}
break;
case LeftSpeedDownButton:
if (speed1 > 100)
{
speed1 -= ChangeSpeed;
Serial.print(millis());
Serial.print("\t");
Serial.println(speed1);
}
break;
case RightSpeedUpButton:
if (speed1 < 254)
{
speed2 += ChangeSpeed;
Serial.print(millis());
Serial.print("\t");
Serial.println(speed2);
}
break;
case RightSpeedDownButton:
if (speed1 > 100)
{
speed2 -= ChangeSpeed;
Serial.print(millis());
Serial.print("\t");
Serial.println(speed2);
}
break;
}
return;
}
void root()
{
save = 0;
Serial.print(millis());
Serial.print("\t");
Serial.print("root available\n");
while (1)
{
int res;
res = Serial.read();
if (res == RootExit)
{
Serial.print(millis());
Serial.print("\t");
Serial.print ("root disable\n");
break;
}
KeySelect(res);
}
return;
}
void connection ()
{
stop1();
Serial.print(millis());
Serial.print("\t");
Serial.print("lost_connection\n");
digitalWrite(39, HIGH);
delay (2000);
digitalWrite(39, LOW);
}
long getEchoTiming()
{
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
long duration = pulseIn(EchoPin, HIGH);
return duration;
}
/*void avto()
{
Serial.print(millis());
Serial.print("\t");
Serial.println("avto_go");
int ran_dom = 0;
long int res = 0;
int rIR = 0;
int lIR = 0;
int mIR = 0;
delay(250);
while (res != StopProgramm ) ////change!
{
if (Serial.available())
{
res = Serial.read();
//Serial.println("zashlo");
}
long distance = getEchoTiming();
rIR = digitalRead(rightIR);
lIR = digitalRead(rearIR);
mIR = digitalRead(frontIR);
//Serial.println(rIR);
//Serial.println(lIR);
//Serial.println(distance);
if ( mIR == 0 )
{
if ( (distance > 250) & (rIR == 1) & (lIR == 1))
{
forward();
//Serial.println("forward");
} else if ((distance > 250) & (rIR == 1) & (lIR == 0))
{
right();
//Serial.println("right");
delay(200);
}
else if ((distance > 250) & (rIR == 0) & (lIR == 1))
{
left ();
//Serial.println("left");
delay(200);
}
else if ((distance < 250) & (rIR == 1) & (lIR == 0))
{
Serial.println("random right");
stop1();
right ();
//Serial.println("right");
ran_dom = random(400, 800);
delay (ran_dom);
}
else
{
Serial.println("random left");
stop1();
//analogWrite(sp1,110);
//analogWrite(sp2,110);
left();
ran_dom = random(400, 800);
delay (ran_dom);
}
}
else
{
Serial.println("back");
back ();
delay (250);
left ();
delay (300);
}
}
stop1();
return;
}*/
void setup()
{
pinMode(frontIR, INPUT);
Serial.begin(9600); // Выставляем скорость COM порта
Wire.begin();
servokurs.attach(KursPin);
servotung.attach(TungPin);
servokurs.write(kurspos);
servotung.write(tungpos);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
pinMode(int1, OUTPUT);
pinMode(int2, OUTPUT);
pinMode(int3, OUTPUT);
pinMode(int4, OUTPUT);
pinMode(sp1, OUTPUT);
pinMode(sp2, OUTPUT);
pinMode(rightIR, INPUT);
pinMode(rearIR, INPUT);
}
void loop()
{
if (Serial.available())
{
int res;
res = Serial.read();
if (res == RootPermission)
{
root();
}
KeySelect(res);
}
porog();
}