LR_c_legacy

#define switchPinA  11
#define switchPinB  12
#define pinAX       A0
#define pinAY       A1
#define pinBX       A2
#define pinBY       A3
#define ledPin      13
#define bounce      32
#define breaking_rc '_'
#define forward_rc  'w'
#define back_rc     's'
#define right_rc    'd'
#define left_rc     'a'
#define forward2_rc '8'
#define back2_rc    '2'
#define right2_rc   '6'
#define left2_rc    '4'
#define gear1_rc    '<'
#define gear2_rc    '>'
#define changmod_rc 'm'
#define changsrv_rc 'z'
int calibrateAX = 512;
int calibrateAY = 512;
int calibrateBX = 512;
int calibrateBY = 512;
int ax = calibrateAX;
int ay = calibrateAY;
int bx = calibrateBX;
int by = calibrateBY;
bool manual = true;
bool servo = false;
bool servofast = false;
char cur_cmd = breaking_rc;
char next_cmd = cur_cmd;
char cur_gear = gear2_rc;
char next_gear = cur_gear;
void setup() {
  pinMode(ledPin, OUTPUT);
  pinMode(switchPinA, INPUT);
  digitalWrite(switchPinA, HIGH);
  pinMode(switchPinB, INPUT);
  digitalWrite(switchPinB, HIGH);
  calibrateAX = analogRead(pinAX);
  calibrateAY = analogRead(pinAY);
  calibrateBX = analogRead(pinBX);
  calibrateBY = analogRead(pinBY);
  Serial.begin(9600);
  while (!Serial);
  while (true) {
    digitalWrite(ledPin, HIGH);
    delay(256);
    digitalWrite(ledPin, LOW);
    delay(256);
    Serial.write('p');
    if (Serial.available()) {
      while (Serial.read() != '1');
      digitalWrite(ledPin, HIGH);
      Serial.write(breaking_rc);
      break;
    }
  }
}
void loop() {
  if (digitalRead(switchPinB) == LOW){
    Serial.write(changsrv_rc);
    servo = !servo;
    while (digitalRead(switchPinB) == LOW);
  }
  if (servo) {
      if (digitalRead(switchPinA) == LOW){
        servofast = !servofast;
        while (digitalRead(switchPinA) == LOW);
      }
      ax = analogRead(pinAX);
      ay = analogRead(pinAY);
      bx = analogRead(pinBX);
      by = analogRead(pinBY);
      /**/ if (ay < calibrateAY - bounce)
        Serial.write(back_rc);
      else if (ay > calibrateAY + bounce)
       Serial.write(forward_rc);
      else if (ax < calibrateAX - bounce)
        Serial.write(left_rc);
      else if (ax > calibrateAX + bounce)
        Serial.write(right_rc);
      else if (by < calibrateBY - bounce)
        Serial.write(back2_rc);
      else if (by > calibrateBY + bounce)
       Serial.write(forward2_rc);
      else if (bx < calibrateBX - bounce)
        Serial.write(left2_rc);
      else if (bx > calibrateBX + bounce)
        Serial.write(right2_rc);
      else 
        Serial.write(breaking_rc);
      if(!servofast)
        delay(16);
  } else {
    if (digitalRead(switchPinA) == LOW) {
      Serial.write(changmod_rc);
      manual = !manual;
      if (manual)
        digitalWrite(ledPin, HIGH);
      else
        digitalWrite(ledPin, LOW);
      while (digitalRead(switchPinA) == LOW);
    }
    if (manual) {
      ax = analogRead(pinAX);
      ay = analogRead(pinAY);
      if ((ax <= calibrateAX - bounce*3) || (ay <= calibrateAY - bounce*3) || (ax >= calibrateAX + bounce*3) || (ay >= calibrateAY + bounce*3))
        next_gear = gear2_rc;
      else
        next_gear = gear1_rc;
      if (next_gear != cur_gear) {
        cur_gear = next_gear;
        Serial.write(cur_gear);
      }
      if (ay < calibrateAY - bounce)
        next_cmd = back_rc;
      else if (ay > calibrateAY + bounce)
        next_cmd = forward_rc;
      else if (ax < calibrateAX - bounce)
        next_cmd = left_rc;
      else if (ax > calibrateAX + bounce)
        next_cmd = right_rc;
      else 
        next_cmd = breaking_rc;
      if (next_cmd != cur_cmd || next_cmd == breaking_rc) {
        cur_cmd = next_cmd;
        Serial.write(cur_cmd);
      }
    }
  }
}

LR_legacy

#include <Servo.h>
#define SPEED_1       5 //left PWM
#define DIR_1         4 //HIGH to forward
#define SPEED_2       6 //right PWM
#define DIR_2         7 //HIGH to forward
#define trigPin_left  3 //HC-SR04 Trig
#define echoPin_left  2 //HC-SR04 Echo
#define trigPin_right 9 //HC-SR04 Trig
#define echoPin_right 8 //HC-SR04 Echo
#define servo1_pin    10//поворот влево вправо
#define servo2_pin    11//вверх вних манипулятор
#define servo3_pin    12//вверх вних зажима
#define servo4_pin    13//сжатие расжатие зажима
#define min_distance  35 //in centimetres
#define gear_sound    70
#define gear_1        250
#define gear_2        250
#define servo1_min     5
#define servo1_max     175 
#define servo2_min     60
#define servo2_max     120 
#define servo3_min     5
#define servo3_max     175 
#define servo4_min     60
#define servo4_max     180 
#define servo_angle    1
#define breaking_rc   '_'
#define forward_rc    'w'
#define back_rc       's'
#define right_rc      'd'
#define left_rc       'a'
#define forward2_rc   '8'
#define back2_rc      '2'
#define right2_rc     '6'
#define left2_rc      '4'
#define gear1_rc      '<'
#define gear2_rc      '>'
#define changmod_rc   'm'
#define changsrv_rc   'z'
#define logging       false
char command = breaking_rc;
bool manual = true;
bool servo = false;
byte cur_speed = gear_2;
int servo1_angle = 90;
int servo2_angle = 90;
int servo3_angle = 90;
int servo4_angle = 90;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
void forward(){
  digitalWrite(DIR_1, HIGH);
  digitalWrite(DIR_2, HIGH);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void breaking(){
  analogWrite(SPEED_1, 0);
  analogWrite(SPEED_2, 0);
}
void left(){
  digitalWrite(DIR_1, HIGH);
  digitalWrite(DIR_2, LOW);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void right(){
  digitalWrite(DIR_1, LOW);
  digitalWrite(DIR_2, HIGH);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void back(){
  digitalWrite(DIR_1, LOW);
  digitalWrite(DIR_2, LOW);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
float distance_left(){
  digitalWrite(trigPin_left, LOW);
  delayMicroseconds(5); //2-5 us
  digitalWrite(trigPin_left, HIGH);
  delayMicroseconds(10); //10 us
  digitalWrite(trigPin_left, LOW);
  return pulseIn(echoPin_left, HIGH) / 58.2;
}
float distance_right(){
  digitalWrite(trigPin_right, LOW);
  delayMicroseconds(5); //2-5 us
  digitalWrite(trigPin_right, HIGH);
  delayMicroseconds(10); //10 us
  digitalWrite(trigPin_right, LOW);
  return pulseIn(echoPin_right, HIGH) / 58.2;
}
void beep(uint16_t sound, uint16_t pause, uint16_t count) {
  for (uint16_t i = 0; i < count; i++){
    analogWrite(SPEED_1, gear_sound);
    analogWrite(SPEED_2, gear_sound);
    delay(sound);
    analogWrite(SPEED_1, 0);
    analogWrite(SPEED_2, 0);
    delay(pause);
  }
}
void setup() {
  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  servo3.attach(servo3_pin);
  servo4.attach(servo4_pin);
  servo1_angle = servo1.read();
  servo2_angle = servo2.read();
  servo3_angle = servo3.read();
  servo4_angle = servo4.read();
  for(int i = 4; i <= 7; i++)    
    pinMode(i, OUTPUT);  
  pinMode(12, INPUT);
  pinMode(trigPin_left, OUTPUT);
  pinMode(echoPin_left, INPUT);
  pinMode(trigPin_right, OUTPUT);
  pinMode(echoPin_right, INPUT);
  randomSeed(analogRead(0));
  if (logging) {
    Serial.begin(9600);
    while(!Serial);
  }
  if (manual) {
    Serial2.begin(9600);
    while(!Serial2);
    while (!Serial2.available());
    if (Serial2.read() == 'p') {
      Serial2.write('1');
      while (Serial2.read() != breaking_rc);
      beep(256, 128, 3);
      beep(128, 64, 3);
    }
  } else
      beep(512, 256, 2);
}
void loop() {
  if (manual) {
    while (Serial2.available()){
      command = Serial2.read();
      if (logging) {
        Serial.print(command);
        Serial.print(" $ ");
        Serial.print(distance_left());
        Serial.print(" - ");
        Serial.println(distance_right());
      }
      if (servo) {
        switch (command) {
          case forward_rc:
            servo1_angle += servo_angle;
            if (servo1_angle > servo1_max)
              servo1_angle = servo1_max;
            servo1.write(servo1_angle);
            break;
          case back_rc:
            servo1_angle -= servo_angle;
            if (servo1_angle < servo1_min)
              servo1_angle = servo1_min;
            servo1.write(servo1_angle);
            break;
          case right_rc:
            servo2_angle += servo_angle;
            if (servo2_angle > servo2_max)
              servo2_angle = servo2_max;
            servo2.write(servo2_angle);
            break;
          case left_rc:
            servo2_angle -= servo_angle;
            if (servo2_angle < servo2_min)
              servo2_angle = servo2_min;
            servo2.write(servo2_angle);
            break;
          case forward2_rc:
            servo3_angle += servo_angle;
            if (servo3_angle > servo3_max)
              servo3_angle = servo3_max;
            servo3.write(servo3_angle);
            break;
          case back2_rc:
            servo3_angle -= servo_angle;
            if (servo3_angle < servo3_min)
              servo3_angle = servo3_min;
            servo3.write(servo3_angle);
            break;
          case right2_rc:
            servo4_angle += servo_angle;
            if (servo4_angle > servo4_max)
              servo4_angle = servo4_max;
            servo4.write(servo4_angle);
            break;
          case left2_rc:
            servo4_angle -= servo_angle;
            if (servo4_angle < servo4_min)
              servo4_angle = servo4_min;
            servo4.write(servo4_angle);
            break;
          case changsrv_rc:
            servo = !servo;
            break;
          default:
            break;
        }
      } else {
        switch (command) {
          case forward_rc:
            forward();
            break;
          case back_rc:
            back();
            break;
          case right_rc:
            right();
            break;
          case left_rc:
            left();
            break;
          case gear1_rc:
            cur_speed = gear_1;
            break;
          case gear2_rc:
            cur_speed = gear_2;
            break;
          case changmod_rc:
            cur_speed = gear_1;
            manual = !manual;
            break;
          case changsrv_rc:
            servo = !servo;
            break;
          default:
            breaking();
            break;
        }
      }
    }
  } else {
    if (Serial2.available())
      if (Serial2.read() == changmod_rc)
        manual = !manual;
    if ((distance_left() < min_distance) || (distance_right() < min_distance)) {
      if (distance_left() >= distance_right())
        left();
      else
        right();
      delay(256);
      breaking();
    } else
      forward();
  }
}