Problem Adding MicroServo to existing Code ERROR

Hello everyone! I am been attempting to integrate a micro servo motor to my object avoiding robot code. I believe I am going in the right direction however, I don’t understand as to why my robot only moves in a circular motion. To better describe the hardware of the robot, I have an ARDUINO MOTOR SHILED mounted onto an ARDUINO UNO. I have two motor wheels connected to inputs A and B on the side of the motor shield. In addition the ultra sonic sensor is connected to the following pin connections:

PIN 6 → ECHO
PIN 7 → TRIG
with PIN 5 → Micro Servo motor

This is the original code used for the object-avoiding robot
// Arduino-Ardumoto obstacle avoiding robot

#include <NewPing.h>

#define TRIGGER_PIN 7 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 6 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum

// sensor distance is rated at 400-1000cm.

#define CW 1
#define CCW 0

#define MOTOR_A 0
#define MOTOR_B 1

// Pin Assignments
// Don’t change these! These pins are statically defined by the shield layout
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13; // Direction control for motor B

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

void setup()
{
// All pins should be set up as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);

// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}

void loop()
{

delay(50);
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).

if ( uS / US_ROUNDTRIP_CM > 20 || uS / US_ROUNDTRIP_CM == 0 )
{

forward(180);

}

else if( uS / US_ROUNDTRIP_CM < 20)

{

turnRight(180);
delay(500);
}

}

void driveArdumoto (byte motor, byte dir, byte spd)
{
if(motor == MOTOR_A)

{
digitalWrite(DIRA,dir);
analogWrite(PWMA,spd);
}

else if(motor == MOTOR_B)
{

digitalWrite(DIRB,dir);
analogWrite(PWMB,spd);
}
}

void forward(byte spd)

{ driveArdumoto(MOTOR_A, CW, spd);
driveArdumoto(MOTOR_B,CW, spd);
}

void turnRight(byte spd)
{
stopArdumoto(MOTOR_B); //Motor B stop
driveArdumoto(MOTOR_A, CCW, spd); //Motor A run
}
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0 ,0);
}

The code works great for the robot however, I am trying to add the use of the micro servo motor onto this code. I have come up with the code below, however the robot no longer avoids objects but instead spins in a circular motion with the ultra sonic sensor looking right to left.

#include<Servo.h>
Servo SM;

#define T 7 /// TRIGGER_PIN 7
#define E 6 //// ECHO ON PIN 6
#define PWMA 3
#define PWMB 11
#define DIRA 12
#define DIRB 13

#define TRIGGER_PIN 0
#define ECHO_PIN 1

void setup() {
// put your setup code here, to run once:
pinMode(T, OUTPUT);
pinMode(E, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);

pinMode(TRIGGER_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);

analogWrite(DIRA, 220);
analogWrite(DIRB, 240);

SM.attach(5);
SM.write(90);
delay(3000);

}
int distance;
void loop() {
// put your main code here, to run repeatedly:
findDistance() ;

if (distance > 20) {
forward();
} else {
delay(60);
findDistance();
if (distance > 20) {
forward();
} else {
Stop();
SM.write(40);
delay(500);
findDistance();
if (distance>20){
SM.write(90);
delay(1000);
rightward();
delay(500);
Stop();
}else{
SM.write(130);
delay(1000);
findDistance();
if (distance>20){
SM.write(90);
delay(1000);
leftward();
delay(500);
Stop();
}else{
SM.write(90);
delay(1000);
rightward();
delay(1500);
Stop();

}
}
}
}
}
void forward() {
digitalWrite(PWMA, HIGH);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, HIGH);
digitalWrite(DIRB, LOW);
}
void backward() {
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, HIGH);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, HIGH);
}
void leftward() {
digitalWrite(PWMA, HIGH);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, HIGH);
}
void rightward() {
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, HIGH);
digitalWrite(DIRA, HIGH);
digitalWrite(DIRB, LOW);
}
void Stop() {
digitalWrite(PWMA, LOW);
digitalWrite(PWMA, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}

void findDistance() {
int duration;
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);

duration = pulseIn(ECHO_PIN, HIGH);
distance = (1 / 29.1) * (duration / 2);
delay(60);

}

I have gone about a good 5 hours trying to see my errors and have started to wonder if I made the right approach in adding the micro servo to my original code. I would gladly appreciate any help. Thanks.

Why did you move the ultrasonic sensor to pins 0 and 1?

Read the how get the most out of this forum sticky to see how to properly post code. Remove useless white space and format the code with the IDE autoformat tool (crtl-t or Tools, Auto Format) before posting code.

Once that you move the sonar off of pins 0 and 1, start the Serial port and use some serial prints to trace the program flow and variable values. Blind troubleshooting without your most valuable debugging aid (serial prints) is very difficult.

And please edit your OP to put the code in code tags. As it is it looks like one long program. The non code part, in the middle, about the changes is easily missed.