Multi-Functional 2WD driving Straight Robot Car

 /*
   tutorial Making a 2WD Arduino Vehicle Drive Straight  available on site  
https://www.hackster.io/Kenan-Paralija/making-a-2wd-arduino-vehicle-drive-straight-ae40ee
   
   use the GY-521 module (MPU-6050 Board) to make a 2WD robotcar drive in a straight
  include variable direktion for direktion correktion
*/

#include "Wire.h"
#include "I2Cdev.h"
#include <SoftwareSerial.h>   //verwende die 'Serial' Library

#include <Servo.h>  //servo library
  Servo myservoH;      // create servo object to control servo
  int posH;
  Servo myservoV;      // create servo object to control servo
  int posV;
  int EndDown=50; //End Position for ServoV
  int EndUp=130;  //End Position for ServoV
  bool schkVU;
  bool schkVD;
  bool schkHR;
  bool schkHL;
  bool mov90 ;
  bool goF;
  bool goB;
  int t=40; //time used to adjust the servo speed
   
#define MPU6050_ADDR 0x68 // Alternatively set AD0 to HIGH  --> Address = 0x69

//define DRIVE module IO Pin
  #define STBY 3  // STBY  <  ENR || ENL 
  #define IN1 8   //Left
  #define ENL 6   //PWML

  #define IN3 7   //Right
  #define ENR 5   //PWMR
 
  int highspeed=255;
  int lowspeed=10;
  int mySpeed = 200;   // speed
  
  int corrSpeedL;       //Left Motor
  int corrSpeedR;       //Right Motor
  char getstr;
 
  //
  int direktion;

  //***** MPU6050 **********
  //A5 - SCL ;  A4 - SDA; interupt 2;
  #define MPU6050_RA_GYRO_ZOUT_H      0x47
  //uint8_t buffer[14];
  float gyroZ; // Raw register values  gyroscope
  
  unsigned long lastTime = 0;
  float dt;      //Differential time
  long gyroZ0;  //Gyro offset = mean value
  float yaw;
  float yawOld;
  float gyroAngleZ; //Angle variable

  

//before execute loop() function, 
void setup() {
  Serial.begin(9600);
  pinMode(IN1,OUTPUT);//before useing io pin, pin mode must be set first 
  pinMode(ENR,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(ENL,OUTPUT);
  
  Wire.begin();
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // wake up!
  Wire.endTransmission(true);

  myservoH.attach(10,700,2400);  // attach servo on pin 10 to servo object
  myservoV.attach(11,700,2400);  // attach servo for Kopf on pin 11 to servo object
  delay(2000);
  Serial.print("   ***   2WD_22 ****");
  
  direktion =5;
  schkVU= LOW;
  schkVD= LOW;
  schkHR= LOW;
  schkHL= LOW;
  mov90 = LOW;
  goF =LOW;
  goB=LOW;
  calibration();
  // Call this function if you need to get the IMU error values for your module
  
}
void loop() { 
    // control getspeed=mySpeed, gerDirection0forward/back
  if(Serial.available())   // if receive the data
  {
    getstr=Serial.read();    // read the received data
      //Serial.print(getstr);
  
    
  switch(getstr){
    
    case 'A': goF=HIGH; break; // execute the corresponding function when receive the value
    //Option: may be followed by a number indicating how much throttle (mySpeed) the corresponding motor should give. 
    case 'D': goB=HIGH;   break;
    case 'B': left(mySpeed);   break;
    case 'C': right(mySpeed);  break;
    case 'E': halt();   break;
    case '1': stp(); schkVU=1; break; // Option kamera svenk up-down
    case '2': stp(); schkHL=1; break; //Option kamera svenk left-right
    case '3': stp(); schkHR=1; break; //Option kamera svenk left-right
    case '4': stp(); schkVD=1; break; // Option kamera svenk up-down
    case '5': stp(); mov90=1; break;  //set Camera Zero Position
    // V or H case string for Servo
    case 'V': posV = Serial.parseInt();movToV();break; //now holds posV
    case 'H': posH = Serial.parseInt();movToH();break;   //now holds posH and mov()
    default:  break;
     }
  }
  
  forward(mySpeed);
  back(mySpeed);
  schvenkV();
  schvenkH();
  mov();  
  }

  //************

   
  
  int16_t getRotationZ() {
    uint8_t buffer[14];
  I2Cdev::readBytes(MPU6050_ADDR, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
  return (((int16_t)buffer[0]) << 8) | buffer[1];
  }
  //routine from MPU6050.cpp
  
    
  void calibration()
 {
  //Here we do 100 readings for gyro sensitiv Z-axis output -gyroZ, we sum them and divide them by 100.
  // gyrZ0 mean value of gyroZ raw register values in 100 ms
  
  unsigned short times = 100; //Sampling times
  for (int i = 0; i < times; i++)
  {
    gyroZ = getRotationZ();     // gyroZ - Raw register values gyroscope Z axis
    gyroZ0 += gyroZ; //sum all measured values gyroZ
  }
  gyroZ0 /= times; 
  }

 void calcYaw(){
  
  unsigned long currentTime = millis();   //current time(ms)
  //get the current timestamp in Milliseconds since epoch time which is
  dt = (currentTime - lastTime) / 1000.0; //Differential time(s)
  lastTime = currentTime;                 //Last sampling time(ms)

  gyroZ = getRotationZ();
  
  float angularZ = (gyroZ - gyroZ0) / 131.0 * dt; //angular z: =t
  if (fabs(angularZ) < 0.05) //
  {
    angularZ = 0.00;
  }
  gyroAngleZ += angularZ; //returns the absolute value of the z-axis rotazion integral 
  yaw = - gyroAngleZ;

  /*
  Serial.print(" | GyZo = "); Serial.print(toStr(gyroZ0));
  Serial.println();
  Serial.print(" | GyroAngle = "); Serial.print (gyroAngleZ);
  Serial.println();
  */
   }

   //++++++++++++++++++++++++
   
 
  void driveStraight(int speed){
  static unsigned long onTime;
   //to compute corrSpeed(A/B) The yaw data is acquired at the first startup, and the data is updated every ten millisecundes.
  if (millis() - onTime > 10){
  corrSpeed(speed);
  onTime = millis();  
 }  
  }
  
  void corrSpeed(int myspeed){
  calcYaw();
  int  kp= 15; ////Add proportional constant - p( ???)
  //if drive direktion changes: corrSpeedA = mySpeedA - (yawOld - yaw) * kp;
  corrSpeedR = mySpeed + (yaw-yawOld)*kp; //maintain speed by speeding up right motor
  if (corrSpeedR > highspeed)
  {
    corrSpeedR = highspeed;
  }
  else if (corrSpeedR < lowspeed)
  {
    corrSpeedR = lowspeed;
  }
  //if drive direktion changes:corrSpeedB = mySpeedB + (yawOld - yaw) * kp;
  corrSpeedL = mySpeed - (yaw-yawOld)*kp; //if error is positive, slow down left motor
  if (corrSpeedL > highspeed)
  {
    corrSpeedL = highspeed;
  }
  else if (corrSpeedL < lowspeed)
  {
    corrSpeedL = lowspeed;
  }
   
 }


   //*****
   void forward(int is_speed){ 
   if (direktion != 1) //
  {
    yawOld = yaw;
  }
  
   if (goF==HIGH ) {
    //go forward
  driveStraight(is_speed);

   //digitalWrite(ENA,HIGH);
  digitalWrite(STBY,HIGH);
  // A ... LEFT
  digitalWrite(IN1,HIGH); //set IN1 hight level enable A channel CW 
  analogWrite(ENL,corrSpeedL);  //set EN for A RIGHT side speed
  //  B ... RIGHT
  digitalWrite(IN3,HIGH);  //set IN3 hight level enable B channel CW 
  analogWrite(ENR,corrSpeedR); //set EN for B LEFT side speed
  //Serial.println("Forward");//send message to serial monitor
  direktion = 1;
    } 

}

void back(int is_speed){
      if (direktion != 4) //
  {
    yawOld = yaw;
  }
  
  if (goB==HIGH ) {
    //go back
  driveStraight(is_speed);
  
  //digitalWrite(ENA,HIGH);
  digitalWrite(STBY,HIGH);
   // A ... LEFT 
  digitalWrite(IN1,LOW); //set IN1 LOW level enable A channel CCW 
  analogWrite(ENL,corrSpeedR);  //set EN for A RIGHT side speed
  //  B ... RIGHT
  digitalWrite(IN3,LOW);  //set IN3 hight level enable B channel CCW 
  analogWrite(ENR,corrSpeedL); //set EN for B LEFT side speed
  //Serial.println("Back");//send message to serial monitor
   direktion = 4;
    } 

}

void left(int is_speed){
  digitalWrite(STBY,HIGH);
  //  A ... LEFT 
  digitalWrite(IN1,LOW); //set IN1 hight level enable 
  analogWrite(ENL,is_speed);  //set EN for A RIGHT side speed
     // B ... RIGHT
  digitalWrite(IN3,HIGH);  //set IN3 hight level enable 
  analogWrite(ENR,is_speed); //set EN for B LEFT side speed
  //Serial.println("Left");
   direktion = 2;
}

void right(int is_speed){
  digitalWrite(STBY,HIGH);
  //  A ... LEFT
  digitalWrite(IN1,HIGH); //set IN1 LOW level enable A channel CCW 
  analogWrite(ENL,is_speed);  //set EN for A RIGHT side speed
   // B ... RIGHT
  digitalWrite(IN3,LOW);  //set IN3 hight level enable B channel CW 
  analogWrite(ENR,is_speed); //set EN for B LEFT side speed
  //Serial.println("Right");
   direktion = 3;
}

void halt(){
     stp();  //all servo stop
   goB=LOW;
   goF=LOW;
  digitalWrite(STBY,LOW); 
  analogWrite(ENL,0);  //set A side speed 0
  
  analogWrite(ENR,0); //set B side speed 0
  Serial.println("stop");

  //kopf();
  direktion = 5;
}

 // ++++++
 void stp() {
  schkVU= LOW;
  schkVD= LOW;
  schkHR= LOW;
  schkHL= LOW;
}

// myservoV for Kopf and schvenk in arbeit
void schvenkH(){
    // camera svenk PANORAM
     // left Right
      posH=myservoH.read();    // read the angle value of each servo
     if(schkHR==HIGH)  // if push 3 the right
    {
      posH--;
      if (posH>10)  myservoH.write(posH);  // servo 1 (H) operates the motion, the head move slowly right
      delay(t);
    }
    
    if(schkHL==HIGH)  // if push the left 2 
    {
      posH++;
      if (posH<170)  myservoH.write(posH);  // servo 1 operates the motion, the head move slowly left
    delay(t);
    }
  }

    
    void schvenkV(){
  // TILT a Head turn  down
    posV= myservoV.read();    // read the angle value of each servo
      if(schkVD==HIGH)  // if push 4 the right
    {
      posV--;
      if (posV>EndDown)   myservoV.write(posV);  // servo 1 (V) operates the motion, 
      //the head move slowly down- min 50
      delay(t);   //t=40ms
    }
    
    if(schkVU==HIGH)  // if push the up 1 
    {
      posV++;
      if (posV<130)      myservoV.write(posV);  // servo 1 operates the motion, 
      //the head move slowly up
    delay(t);
    }
     }
  void mov()
  { 
    if (mov90==HIGH)
    {
    //pos V ------
    myservoV.write(90);
    

    //pos H-----------
    myservoH.write(90);
    mov90=LOW;
   }
  }

void movToV()
  { 
    //++++++++++++++ A Head Vertikal turn to posV
      //stp();
      if (posV<EndDown)  posH=EndDown;
      if (posV>EndUp)  posH=EndUp;
    myservoV.write(posV);
    delay(t);      // delay 40/5msused to adjust the servo speed    
  }
     
  void movToH()
  { 
    // ++++++++++++ A Head Horizontal turn to posH
      //stp();
      if (posH<10)  posH=10;
      if (posH>170)  posH=170;
    myservoH.write(posH);
    delay(t);      // delay 20/5msused to adjust the servo speed    
  }