Asking a servo to sweep on an obstacle avoidance car? Answered

I am just a few weeks into learning how these things work, and as such, have primarily been looking at coding from examples and getting them to fit with what I am doing.  Here's the setup.  We (my students and I) have built an elegoo 4 wheeled arduino platform car.  Primarily, we are using it to autonomously move around the classroom, however we've found that the ultrasonic sensor just isn't cutting it.  We've added an optical sensor, which helps, and I've figured out the coding on getting that working.  Now, we are looking at having the servo holding the ultrasonic sensor to sweep 45 degrees to the left, then 45 to the right, and repeat.  We feel that we may be able to prevent some wall crashes and chair crashes that happen when the car comes in at an angle.  Here is the code we have now.

The ABS and ACS numbers are different to account for the variances in motor manufacturing (the car wouldn't drive straight if all motors were spun up at the same speed).  We do have a second arduino hooked up mainly to control the LED's the kids added, but that code and board does not affect anything for this code and board.


#include <Servo.h>
Servo myservo;
int Echo = A4; 
int Trig = A5;
int in1 = 6;
int in2 = 7;
int in3 = 8;
int in4 = 9;
int ENA = 11;
int ENB = 5;
int ABS = 120;
int ACS = 115;
int angle = 0;
const int avoidPin = 10;
int rightDistance = 0,leftDistance = 0,middleDistance = 0 ;



void _mForward()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);

Serial.println("go forward!");
}



void _mBack()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
Serial.println("go back!");
}

void _mright()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
Serial.println("go right!");
}

void _mleft()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ACS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
Serial.println("go left!");
}

void _mStop()
{
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);
  Serial.println("Stop!");
}

int Distance_test()  
{
  digitalWrite(Trig, LOW);  
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);  
  float Fdistance = pulseIn(Echo, HIGH); 
  Fdistance= Fdistance/58;      
  return (int)Fdistance;
} 

void setup()
{
  myservo.attach(3);
  Serial.begin(38400);    
  pinMode(Echo, INPUT);   
  pinMode(Trig, OUTPUT);
  pinMode(avoidPin, INPUT); 
  pinMode(in1,OUTPUT);
  pinMode(in2,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
   _mStop();
}

void loop()
{
  boolean avoidVal = digitalRead (avoidPin);

    myservo.write(90);
    delay(500);
    middleDistance = Distance_test();
    #ifdef send
    Serial.print("middleDistance=");
    Serial.println(middleDistance);
    #endif

    if((middleDistance<=50) || (avoidVal==LOW))
    {    
      _mStop();
      delay(500);                        
      myservo.write(5);         
      delay(1000);     
      rightDistance = Distance_test();

      #ifdef send
      Serial.print("rightDistance=");
      Serial.println(rightDistance);
      #endif

      delay(500);
       myservo.write(90);             
      delay(1000);                                                 
      myservo.write(180);             
      delay(1000);
      leftDistance = Distance_test();

      #ifdef send
      Serial.print("leftDistance=");
      Serial.println(leftDistance);
      #endif

      delay(500);
      myservo.write(90);             
      delay(1000);
      if(rightDistance>leftDistance) 
      {
        _mright();
        delay(180);
       }
       else if(rightDistance<leftDistance)
       {
        _mleft();
        delay(180);
       }
       else if((rightDistance<=50)||(leftDistance<=50) || (avoidVal=LOW))
       {
        _mBack();
        delay(180);
       }
       else
       {
        _mForward();
       }
    } 
    else
        _mForward();                    
}