Robo wiki

#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();
    }
 
    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 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();
}