Need help with connecting 2 servos together

[squat]

I want the servos (I have 2) to connect together, and to do the same function. I don't know the first thing about coding especially c++, but maybe there's a way to combine the code?

// www.elegoo.com

//
// Include 'header' files (.h files) that contain necessary software libraries:
//
#include <Adafruit_MotorShield.h>  // Libary for motor shield
#include <Servo.h>                 // Library for controlling servo motor
#include <NewPing.h>               // Library for controlling the ultrasonic sensor


//
// Define pins for ultrasonic sensor:
//
#define TRIG_PIN   10  // Define the trigger pin as analog pin #0  
#define ECHO_PIN    9 // Define the echo pin as analog pin #1
#define RIGHT_BLINK_PIN 1 //(TEST) LED indicator for right
//
// Define pin for servo motor:
//
#define SERVO_PIN  6  // Defines the pin to use for the servo to pin #9, which is connected to
                      // pins labelled 'SERVO_2' on the Motor Drive Shield
#define green_LED 2
#define red_LED 4
#define yellow_LED 3
//
// Create software 'objects' (part of 'object-oriented programming'):
//
Servo       myservo;    // Create servo object to control servo motor. 
Adafruit_MotorShield myMotorShield = Adafruit_MotorShield(); // Create motor-shield object with the default I2C address

//
// Assign Adafruit_DCMotor objects to the 4 'ports' (connections) on the 
// motor shield. The numbering of these motors assumes that the motors on left of car are 
// wired to 'M1' and 'M2' on the Motor Shield, and the motors on right of car are wired to
// 'M3' and 'M4'. (It's OK to deviate from this convention, but you'll have to modify the 
// 'turn_left' and 'turn_right' functions below.)
//
Adafruit_DCMotor *Motor_A = myMotorShield.getMotor(1); // Assign 'Motor_A' to 'port' M1 on motor shield
Adafruit_DCMotor *Motor_B = myMotorShield.getMotor(2); // Assign 'Motor_B' to 'port' M2 on motor shield
Adafruit_DCMotor *Motor_C = myMotorShield.getMotor(3); // Assign 'Motor_C' to 'port' M3 on motor shield
Adafruit_DCMotor *Motor_D = myMotorShield.getMotor(4); // Assign 'Motor_D' to 'port' M4 on motor shield


// Create a 'sonar' object, in which we define which Arduino pins to use as the trigger pin 
// and the echo pin. (Pin numbers are defined above.)
NewPing     sonar(TRIG_PIN, ECHO_PIN);  

// Define the speed for car when travelling straight (on scale from 0 to 255). Car's actual
// speed will depend on the battery power and the floor surface (tile vs carpet).
int carSpeed = 75; 

// Define speed for turning car (on scale from 0 to 255). This parameter and the 'turningDuration'
// parameter (below) will determine how far the car turns.
int turningSpeed = 200;

// Define the duration for turning the car, in milliseconds. Lengthen the delay time 
// to turn more; shorten time to turn less. 
int turningDuration = 500; 

// Define variables that will contain the distances sensed to obstacles by the
// ultrasonic sensor:
int straightDistance    ;  // Distance to obstacle straight ahead (in cm)
int rightDistance       ;  // Distance to obstacle 90 degrees to right
int leftDistance        ;  // Distance to obstacle 90 degrees to left
int rightCenterDistance ;  // Distance to obstacle 45 degrees to right
int leftCenterDistance  ;  // Distance to obstacle 45 degrees to left


//==============================================================================================
//==============================================================================================
//
// Run the 'setup' function at the beginning of the program:
//
//==============================================================================================
//==============================================================================================
void setup() { 

  Serial.begin(9600);  // Starts the serial communication at the specified baud rate
  Serial.println("============================================================");
  Serial.println("================ Starting the program ======================");
  Serial.println("============================================================");

  myservo.attach(SERVO_PIN); // Attaches the servo pin to the servo object (SERVO_2 on the Motor Drive Shield)
  myservo.write(90);         // Tells the servo to position at 90-degrees (straight ahead).
  pinMode (green_LED, OUTPUT);
  pinMode (red_LED, OUTPUT);
  pinMode (yellow_LED, OUTPUT);
  myMotorShield.begin();  // Start the motor_shield with the default PWM frequency (1.6 kHz)

  stop();   // Initially, stop all the DC motors
} 

//==============================================================================================
//
// Run the 'loop' function continuously:
//
//==============================================================================================
void loop() { 

    // Check for obstacles ahead:
  check_for_obstacles_ahead();
  
  //
  // Check that there is sufficient space directly ahead and 45 degrees to each side:
  //
  if ((straightDistance > 30) && (rightCenterDistance > 20) && (leftCenterDistance > 20)) 
  {
    // Obstacles in all 3 directions are far away, so move forward:
    digitalWrite (green_LED, LOW);
    digitalWrite (red_LED, LOW);
    digitalWrite (yellow_LED, LOW);
    go_forward(carSpeed);
  }

  else
  { 
    // Obstacles are too close, so ensure that motors are stopped, then back up
    // slightly, and then determine which direction to turn the car:
    stop();
    Serial.println("========== Too close to obstacles in front. ================");
    go_backward(carSpeed);
    digitalWrite (green_LED, HIGH);
    delay(100);   // Go backwards for only 100 ms
    stop();

    // Check distances to left and right:
    check_for_obstacles_left_and_right(); 

    // Turn car towards direction that has more open space:
    turn_towards_open_space();
    
  }   // End of if/else block for (straightDistance > 30) && (rightCenterDistance > 20) && (leftCenterDistance > 20)

}  // End of 'loop' function


// ==============================================================
// ========== Below are where we define the functions ===========
// ========== that are called above in the 'setup'    ===========
// ========== and 'loop' functions.                   ===========
// ==============================================================

// ==============================================================
// Function 'set_speed' sets the 4 motors to the speed given by 
// 'speed', which can range from 0 to 255:
//
void set_speed(int speed)
{
  // Serial.print("Setting motor speed to: ");
  // Serial.println(speed);
  Motor_A->setSpeed(speed);
  Motor_B->setSpeed(speed);
  Motor_C->setSpeed(speed);
  Motor_D->setSpeed(speed);
}  // End of function 'set_speed'

// ==============================================================
// Function 'go_forward' sets the direction to forward for all
// 4 motors:
//
void go_forward(int speed)
{
  Serial.println("Going forward...");
  set_speed(speed);
  Motor_A->run(FORWARD);
  Motor_B->run(FORWARD);
  Motor_C->run(FORWARD);
  Motor_D->run(FORWARD);
}  // End of function 'go_forward'

// ==============================================================
// Function 'go_backward' sets the direction to backward for all
// 4 motors:
//
void go_backward(int speed)
{
  Serial.println("Going backward...");
  set_speed(speed);
  Motor_A->run(BACKWARD);
  Motor_B->run(BACKWARD);
  Motor_C->run(BACKWARD);
  Motor_D->run(BACKWARD);
}  // End of function 'go_backward'


// ==============================================================
// Function 'turn_right' sets the direction to forward for Motor1 and
// Motor2 (on left of car), and to backward for Motor3 and Motor4
// (on the right of car):
//
void turn_right(int speed)
{
  Serial.println("Turning right...");
  set_speed(speed);
  Motor_A->run(FORWARD);
  Motor_B->run(FORWARD);
  Motor_C->run(BACKWARD);
  Motor_D->run(BACKWARD);
}  // End of function 'turn_right'

// ==============================================================
// Function 'turn_left' sets the direction to backward for Motor1 and
// Motor2 (on left of car), and to forward for Motor3 and Motor4 
// (on the right of car):
//
void turn_left(int speed)
{
  Serial.println("Turning left...");
  set_speed(speed);
  Motor_A->run(BACKWARD);
  Motor_B->run(BACKWARD);
  Motor_C->run(FORWARD);
  Motor_D->run(FORWARD);
}  // End of function 'turn_left'


// ==============================================================
// Function 'stop' turns off all 4 motors:
//
void stop() {
  Serial.println("Stopping all motors.");
  set_speed(0);
  Motor_A->run(RELEASE);
  Motor_B->run(RELEASE);
  Motor_C->run(RELEASE);
  Motor_D->run(RELEASE);
}  // End of function 'stop'

// ==============================================================
// Function 'get_distance' returns the distance (in cm) to an
// obstacle, as measured by the ultrasonic sensor:
//
int get_distance() {
  // Create a "long" integer variable to accommodate a large number of microseconds:
  unsigned long duration = 0; 

  // Create a floating-point variable for the distance:
  float float_distance = 0.0;

  // Read the ultrasonic sensor and return the sound wave travel time in microseconds.
  // The 'ping_median()' method will quickly send 5 pings and return the median echo time, 
  // thereby discarding values that deviate from the average/median. 
  duration = sonar.ping_median(10);
  
  // Check if the duration is zero. Sometimes the sensor reports zero microseconds, which is 
  // physically impossible. In these (rare) cases, substitute 1824 microseconds for the echo
  // time, corresponding to an obstacle distance of ~31 cm:
  if (duration == 0)  {
    duration = 1824;
    Serial.println("*** WARNING: Substituted 1824 microseconds for a physically impossible zero-microsecond echo time. ***");
  }
    
  // Prints the round-trip delay in microseconds:
  // Serial.print("||| In function get_distance: Round-trip delay: ");
  // Serial.print(duration);
  // Serial.println(" microseconds");
  
  float_distance = duration *0.034/2;    // Calculate distance in centimeters
 
  // Convert the floating-point distance to an integer:
  int distance = (int)float_distance;

  // Prints the distance on the Serial Monitor:
  // Serial.print("||| In function get_distance: Distance: ");
  // Serial.print(distance);
  // Serial.println(" cm");

  return distance;    // Return the distance to the calling program.
}  // End of function 'get_distance'

// ==============================================================
// Function 'check_for_obstacles_ahead' measures the distances to
// obstacles straight ahead, and 45 degrees to each side. The function
// populates the following three variables with the distance (in cm) to 
// obstacles: straightDistance, rightCenterDistance, and leftCenterDistance.
// Incidentally, if these distances are less than 30cm, 20cm, or 20cm 
// (respectively), this function will stop the car immediately.
//
void check_for_obstacles_ahead()  {
  Serial.println("");  
  Serial.println("=========== Scanning straight and 45 degrees to either side ============");
  
  // Get distance to obstacles straight ahead:
  myservo.write(90);  // Set servo motor to point straight ahead (angle = 90 degrees)
  delay(300);   // Wait for servo motor to stabilize
  straightDistance = get_distance();
  Serial.print("straightDistance = "); Serial.print(straightDistance); Serial.println(" cm.");

  // If obstacle ahead is less than 30 cm, then stop motors immediately:
  if (straightDistance <= 25 )
  {
    stop();
  }

  // Get distance to obstacles 45 degrees to right:
  myservo.write(30);  // Set servo motor to point 45 degrees to right (angle = 45 degrees)
  delay(450);  // Wait for servo motor to stabilize
  rightCenterDistance = get_distance();
  Serial.print("rightCenterDistance = "); Serial.print(rightCenterDistance); Serial.println(" cm.");

  // If obstacle towards right is within 20 cm, then stop motors immediately:
  if (rightCenterDistance <= 25)
  {
    stop();
  }

  // Swing servo back straight ahead and again get distance to obstacles:
  myservo.write(90);  // Set servo motor to point straight ahead (angle = 90 degrees)
  delay(450); 
  straightDistance = get_distance();
  Serial.print("straightDistance = "); Serial.print(straightDistance); Serial.println(" cm.");

  // If obstacle straight ahead is within 30 cm, then stop motors immediately:
  if (straightDistance <= 25)
  {
    stop();
  }

  // Get distance to obstacles 45 degrees to left:
  myservo.write(135);  // Set servo motor to point 45 degrees to left (angle = 135 degrees)
  delay(450); // Wait for servo motor to stabilize
  leftCenterDistance = get_distance();
  Serial.print("leftCenterDistance = "); Serial.print(leftCenterDistance); Serial.println(" cm.");

  // If obstacle towards left is within 20 cm, then stop motors immediately:
  if (leftCenterDistance <= 20)
  {
    stop();
  }
  
  // Again, get distance to obstacles straight ahead:
  myservo.write(90);  // Set servo motor to point straight ahead (angle = 90 degrees)
  delay(300);   // Wait for servo motor to stabilize
  straightDistance = get_distance();
  Serial.print("straightDistance = "); Serial.print(straightDistance); Serial.println(" cm.");

  // If obstacle is within 30 cm straight ahead, then stop motors immediately:    
  if (straightDistance <= 35)
  {
    stop();
  }
}  // End of function 'check_for_obstacles_ahead()'


// ==============================================================
// Function 'check_for_obstacles_left_and_right' measures the distances to
// obstacles 90 degrees to the left and right. The function populates the 
// following two variables with the distance (in cm) to obstacles: 
// leftDistance and rightDistance.
//
void check_for_obstacles_left_and_right()  { 
  // 
  // Turn the sensor 90 degrees left and right to check for obstacles:
  Serial.println("Checking left and right.");
  delay(500);                         
  myservo.write(80); // Turn ultrasonic sensor 90 degrees to right
  delay(1000);      
  rightDistance = get_distance();
  Serial.print("rightDistance = "); Serial.print(rightDistance); Serial.println(" cm.");
      
  delay(500);
  myservo.write(80);              
  delay(1000);                                                  
  myservo.write(180); // Turn ultrasonic sensor 90 degrees to left
  delay(1000); 
  leftDistance = get_distance();
  Serial.print("leftDistance = "); Serial.print(leftDistance); Serial.println(" cm.");
      
  delay(500);
  myservo.write(80);   // Turn ultrasonic sensor straight ahead again            
  delay(1000);
}  // End of function 'check_for_obstacles_left_and_right()'


// ==============================================================
// Function 'turn_towards_open_space' turns the car left or right based
// on the values in the variables 'leftDistance' and 'rightDistance'.
// If leftDistance is larger than rightDistance (i.e., there's more open
// space to the left), then this function turns the car towards the left.
// The opposite occurs if rightDistance > leftDistance.
//
void turn_towards_open_space()  {
  //
  // Determine whether obstacles are closer on the right or on the left. 
  // Turn in the direction that has more room:
  //
  if(rightDistance > leftDistance)
  {
    // There's more room towards the right, so turn in that direction:
    Serial.println("rightDistance > leftDistance, so turning right.");
    digitalWrite(red_LED, HIGH);
    delay(150);
    digitalWrite(red_LED, LOW);
    delay(150);
    digitalWrite(red_LED, HIGH);
    delay(1000);
    digitalWrite(red_LED, LOW);
    turn_right(turningSpeed);
    delay(turningDuration); // Lengthen the delay time to turn more; shorten time to turn less.
    stop();
  }
      
  else  
  {
    // There's more room towards the left, so turn in that direction:
    Serial.println("leftDistance >= rightDistance, so turning left.");
    digitalWrite(yellow_LED, HIGH);
    delay(150);
    digitalWrite(yellow_LED, LOW);
    delay(150);
    digitalWrite(yellow_LED, HIGH);
    delay(1000);
    digitalWrite(yellow_LED, LOW);
    turn_left(turningSpeed);
    delay(turningDuration); // Lengthen the delay time to turn more; shorten time to turn less.
    stop();
  }   // End of if/else block for (rightDistance > leftDistance)

}  // End of function 'turn_towards_open_space()'

// ==============================================================

[DaveX]

Multi-tasking the Arduino - Part 1

[alto777]

@squat

to do the same function.

Just wire both servo control inputs to the pin where you attached the servo in the code.

The two servos will perform identical movements.

a7

[groundFungus]

Please read the how to get the most from the forum posts to see how to properly post code and some information on making a good post.

[squat]

so I plugged one motor into servo 2 because that is what the code connects to, I connect the other one into servo one and its not doing what I want it to

[jim-p]

Then it's the code but we will never know because you haven't followed the advice given in post #4

[squat]

Jim it’s going to be ok.

[bluejets]

wow...what a mess.
As far as I can see you have just one servo but what pin you connect to is a mystery.

@squat

#define SERVO_PIN 6 // Defines the pin to use for the servo to pin #9, which is connected to
// pins labelled 'SERVO_2' on the Motor Drive Shield

Might be a good time to (as well as suggested #6) to supply a drawn out circuit diagram (no fritzing thanks)

[TomGeorge]

Hi, @squat
Welcome to the forum.

How to get the best out of this forum

See also FAQ - Arduino Forum for general rules on forum behavior and etiquette. Hello, Welcome to the Arduino Forum. This guide explains how to get the best out of this forum. Please read and follow the instructions below. Being new here you might think this is having rules for the sake of rules, but that is not the case. If you don’t follow the guidelines all that happens is there is a long exchange of posts while we try to get you to tell us what we need in order to help you, which is frus…

Just write some code to control your servos, before adding the rest of your code.

Tom...

[hallowed31]

@squat

Jim it’s going to be ok.

@jim-p just fine; in fact, he's among the best in the forum. How's your project coming along?

Like any online community, there's a culture. Jim was trying to help you. What Jim knows is this: unless you follow the advice given in post #4 by @groundFungus, also among the gurus here along with Jim and the post authored by @PerryBebbington, you'll be awfully lucky if anyone extends the proverbial olive branch to fix your project and simultaneously help you become better at Arduino, which is difficult enough to learn.

We aren't Arduino customer service. The best in this forum include all sorts of engineer types, some of whom have been working with electronics for probably longer than you've been alive. Fwiw, I'm still a noob at 10 years in compared to the real gurus here.

Now I don't like to speak out of turn and I certainly don't speak for Jim. In fact, all I know of him is his contributions on this forum and I don't mind telling you that you're burning a bridge speaking to a respected forum member like that. So why don't you try showing a little effort and meet us halfway with your project dilemma - just drop the ego and follow his advice.

Do that, and I'll delete this post because I want the community to grow and don't want you to feel embarrassed out of the gate.

Welcome aboard? Your move.

[squat]

Thank you