Adding servo(SG90)

kenobi777 · November 21, 2023, 3:28am

I'm currently trying to add servo(SG90) to my robot. The robot is controlled by PS4 controller via bluetooth.

A library called is already included and this is the movement of servo that I want (constantly moving at the range of 30 degree to 150 degree and the servo's speed is at 40)

    const int Max= 150;   //Max 150 degree
    const int Min= 30;    //Min 30 degree

    for(int i= Min; i<=Max; i+=30){
      myservo.write(i,speed,true);
    }
    for(int j= Max; j>=Min; j-=30){
      myservo.write(j,speed,true);
    }

And this is the loop operation. I'm using PS4 stick to control the robot.

void loop() {
  Usb.Task();

  if (PS4.connected()) {

    int Lx;
    Lx= PS4.getAnalogHat(LeftHatX)-127;

    int Ly;
    Ly= 127-PS4.getAnalogHat(LeftHatY);

    double degree;
    degree= atan2(Ly,Lx)*180.0/PI+180;

    double radius;
    radius = sqrt(pow(Lx,2)+pow(Ly,2));
    
    if(radius>=45)
    {
      //*** go straight ***//
      if ((degree>225)&&(degree<315)) 
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("go straight!");

        digitalWrite(5,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(3,HIGH);
        digitalWrite(2,LOW);
      }

      //*** move backward ***//
      if ((degree>45)&&(degree<135))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("move backward");

        digitalWrite(5,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(3,LOW);
        digitalWrite(2,HIGH);
      }

      //*** turn LEFT ***//
      if (((degree>0)&&(degree<45))||((degree>315)&&(degree<360)))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("turn LEFT!!!");

        digitalWrite(5,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(3,HIGH);
        digitalWrite(2,LOW);
      }

      //*** turn RIGHT ***//
      if ((degree>135)&&(degree<225))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("turn RIGHT!!!");

        digitalWrite(5,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(3,LOW);
        digitalWrite(2,HIGH);
      }
    
    //*** STOP ***//
    else
    {
      Serial.println("STOP!");

      digitalWrite(5,LOW);
      digitalWrite(4,LOW);
      digitalWrite(3,LOW);
      digitalWrite(2,LOW);
    }
    }
  }
}

My ideal is to let the servo rotates constantly while the robot is controlled by PS4 stick. But here are the problems. When I put servo operation before if() operation in the loop, the robot was not reacting and only the servo was moving.

Then, I put servo operation into if() operation. For example,

        digitalWrite(5,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(3,HIGH);
        digitalWrite(2,LOW);
     
        for(int i= Min; i<=Max; i+=30){
      myservo.write(i,speed,true);
    }
    for(int j= Max; j>=Min; j-=30){
      myservo.write(j,speed,true);
    }

However, I faced the connection problem. (PS4 is not connected to the arduino at all)

Can somebody help me solve this? Thank you.

xfpd · November 21, 2023, 3:45am

Post your complete sketch to allow reference.

jremington · November 21, 2023, 3:50am

Most of the servo problems reported on this forum stem from trying to use the Arduino 5V output as motor power supply. If you are one of those, use a separate power supply, and connect the grounds. A 4xAA battery pack will work for 1 or 2 small servos.

kenobi777 · November 21, 2023, 4:24am

Here

/*
 Example sketch for the PS4 Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */
#include <PS4BT.h>
#include <usbhub.h>

#include <FlexiTimer2.h>
#include <VarSpeedServo.h>

#include <math.h> 
#define PI 3.141592653589793

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>

USB Usb;
//USBHub Hub1(&Usb); // Some dongles have a hub inside
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so

/* You can create the instance of the PS4BT class in two ways */
// This will start an inquiry and then pair with the PS4 controller - you only have to do this once
// You will need to hold down the PS and Share button at the same time, the PS4 controller will then start to blink rapidly indicating that it is in pairing mode
PS4BT PS4(&Btd, PAIR);

// After that you can simply create the instance like so and then press the PS button on the device
//PS4BT PS4(&Btd);

bool printAngle, printTouch;
uint8_t oldL2Value, oldR2Value;

VarSpeedServo myservo;
int speed= 40;   //servo speed

#define TRG 9
#define ECH 10
#define HIGHTIME 10

// float measure(){                
//   int diff;
//   float dis;
//   digitalWrite(TRG, HIGH);
//   delayMicroseconds(HIGHTIME);
//   digitalWrite(TRG, LOW);
//   diff = pulseIn(ECH, HIGH);
//   dis = (float)diff * 0.01715;
//   return dis;
// }

// void Back{
//   digitalWrite(5,LOW);
//   digitalWrite(4,HIGH);
//   digitalWrite(3,LOW);
//   digitalWrite(2,HIGH);
// }

// void distance(){             //if the distance is below 10cm
//   int loopdis;
//   int newloopdis;

//   loopdis= measure();        //if it's below 10cm, light up LED
//   if(loopdis<10){
//     while(true){
//     //Back();
//     Serial.println("sencor is moving");
//     digitalWrite(7,HIGH);
//     delay(500);
//     digitalWrite(7,LOW);
//     newloopdis= measure();
//     if(newloopdis>10){
//       break;
//     }
//     }
//   }
// }

void setup() {
  Serial.begin(115200);

  myservo.attach(8);   

  pinMode(5,OUTPUT);   
  pinMode(4,OUTPUT);  
  pinMode(3,OUTPUT);   
  pinMode(2,OUTPUT);   

  pinMode(TRG, OUTPUT);
  pinMode(ECH, INPUT);

  pinMode(7, OUTPUT);  

  // FlexiTimer2::set(500,Servo);   
  // FlexiTimer2::start();

#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); // Halt
  }
  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
}



void loop() {
  Usb.Task();

  if (PS4.connected()) {
    const int Max= 150;  
    const int Min= 30;    

    for(int i= Min; i<=Max; i+=30){
      myservo.write(i,speed,true);
      
    }
    for(int j= Max; j>=Min; j-=30){
      myservo.write(j,speed,true);
      
    }

    int Lx;
    Lx= PS4.getAnalogHat(LeftHatX)-127;

    int Ly;
    Ly= 127-PS4.getAnalogHat(LeftHatY);

    double degree;
    degree= atan2(Ly,Lx)*180.0/PI+180;

    double radius;
    radius = sqrt(pow(Lx,2)+pow(Ly,2));
    
    if(radius>=45)
    {
      //*** forward ***//
      if ((degree>225)&&(degree<315)) 
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("go straight!");

        digitalWrite(5,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(3,HIGH);
        digitalWrite(2,LOW);

      }

      //*** backward ***//
      if ((degree>45)&&(degree<135))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("move backward");

        digitalWrite(5,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(3,LOW);
        digitalWrite(2,HIGH);

        
      }

      //*** left ***//
      if (((degree>0)&&(degree<45))||((degree>315)&&(degree<360)))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("turn LEFT!!!");

        digitalWrite(5,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(3,HIGH);
        digitalWrite(2,LOW);

        
      }

      //*** right ***//
      if ((degree>135)&&(degree<225))
      {
        Serial.print(F("\r\ndegree: "));
        Serial.print(degree);
        Serial.println("turn RIGHT!!!");

        digitalWrite(5,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(3,LOW);
        digitalWrite(2,HIGH);

      }
    
    //*** stop ***//
    else
    {
      Serial.println("STOP!");

      digitalWrite(5,LOW);
      digitalWrite(4,LOW);
      digitalWrite(3,LOW);
      digitalWrite(2,LOW);

    }
    }
  }
}

kenobi777 · November 21, 2023, 4:27am

I'm using 9v battery for the arduino, and 3xAA battery pack for the circuit

xfpd · November 21, 2023, 5:33am

If this worked before you added the servo, then the servo is the problem, which includes power required to make the servo work. Have you run the servo alone?

kenobi777 · November 21, 2023, 6:49am

Maybe I should explain with more details. So I'm trying to add collision detection to my robot.(using SG90 and HY-SRF05) And it worked out perfectly when I tested it alone.

This is the code of collision detection (servo will move constantly from 30° to150° at speed of 40, and if HY-SRF05 detect something within 10cm in that range, LED will light up)

#include <FlexiTimer2.h>
#include <VarSpeedServo.h>


VarSpeedServo myservo;
int speed= 40;   

#define TRG 9
#define ECH 10
#define HIGHTIME 10

float measure(){                
  int diff;
  float dis;
  digitalWrite(TRG, HIGH);
  delayMicroseconds(HIGHTIME);
  digitalWrite(TRG, LOW);
  diff = pulseIn(ECH, HIGH);
  dis = (float)diff * 0.01715;
  return dis;
}

void distance(){            
  int loopdis;
  int newloopdis;

  loopdis= measure();       
  if(loopdis<10){
    while(true){
    digitalWrite(7,HIGH);
    delay(500);
    digitalWrite(7,LOW);
    newloopdis= measure();
    if(newloopdis>10){
      break;
    }
    }
  }
}

void setup() {
  myservo.attach(8);

  pinMode(TRG, OUTPUT);
  pinMode(ECH, INPUT);

  pinMode(7, OUTPUT);   

  FlexiTimer2::set(500,distance);   //
  FlexiTimer2::start();
}

void loop() {
  const int Max= 150;   
  const int Min= 30;    

  for(int i= Min; i<=Max; i+=30){
    myservo.write(i,speed,true);
  }
  for(int j= Max; j>=Min; j-=30){
    myservo.write(j,speed,true);
  }

}

So basically I'm trying to figure out how to combine this operation to my robot's movement code that I previously sent you.

kenobi777 · November 21, 2023, 6:56am

I have been working on this problem since I posted this topic. But I came to wonder if it is possible that I add another timer interruption for the servo?

Which means there will be two timer interruptions (one for HY-SRF05 and the other for SG90) while moving the robot. (loop will only remain the code for robot's movement )

xfpd · November 21, 2023, 2:59pm

Start with disconnecting all your external devices, just running the Arduino. Make a sketch that uses your FlexiTimer2 routine to call another function, but with void loop() using blocking code, for example:

#include "FlexiTimer2.h"

void setup() {
  Serial.begin(9600);
  FlexiTimer2::set(500,sayHello); // call sayHello every 500ms
  FlexiTimer2::start();
}

void loop() {
  delay(2000);
  Serial.println("\nLOOP");
}

void sayHello() {
  Serial.print("Jello, Swirl! ");
}

You should get:

LOOP
Jello, Swirl! Jello, Swirl! Jello, Swirl! Jello, Swirl! 
LOOP

Then add the SRF05 hardware and software to the sketch... and verify it works.
Then the Servo.
Then use both.

You might find the servo causing problems.

jremington · November 21, 2023, 4:01pm

That would provide 4.5V at at best. The servo is specified to run on 4.8 to 6V.

kenobi777 · November 24, 2023, 2:40am

So when I tested the sketch you sent me, it worked out perfectly. Then I tried using FlexiTimer2 with HY-SRF05, it also went well. However, when I tried to tested it with my robot, I faced an error.

Therefore, I went through the process again, but everything seemed well before I implement HY-SRF05 to the robot. That's when I figured out the problem. The test went well (like you said) when arduino is disconnected with all my external devices. But an error occurred when I connect the arduino with USB Host Shield 2.0 (for connecting PS4 controller via bluetooth) and test the code.

kenobi777 · November 24, 2023, 2:48am

Here is the code.

#include <FlexiTimer2.h>

#define TRG 9
#define ECH 10


float measure(){
  int diff;
  float dis;

  digitalWrite(TRG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRG, LOW);
  diff = pulseIn(ECH, HIGH);
  dis = (float)diff * 0.01715;
  return dis;
}


void distance(){
  int loopdis;

  loopdis= measure();
  if(loopdis<10){
    Serial.print(loopdis);
    Serial.println(" cm");
  }
  else{
    Serial.println("continue");
  }
}


void setup() {
  Serial.begin(9600);

  pinMode(TRG, OUTPUT);
  pinMode(ECH, INPUT);

  FlexiTimer2::set(500, distance);
  FlexiTimer2::start();
}


void loop() {
  Serial.println("hello!");
  delay(1000);
}

When I only used arduino for the test, the result when as I expected. However, when I added usb host shield on the arduino, loop operation went as usual but the detection of HY-SRF05 only shows 0cm on Serial Monitor.

kenobi777 · November 24, 2023, 2:54am

I'm thinking about using FlexiTimer2 (timer interruption) for SG90's operation. What do you think?

xfpd · November 24, 2023, 4:36am

I think the FlexiTimer2 library uses a timer interrupt, so regardless of the code, FlexiTimer2 will run the stated ISR. See post #9, the blocking code in void loop()... and the interrupt routine (sayHello) still worked.

kenobi777 · November 24, 2023, 4:37am

It worked out with pin 12 and pin 13.

kenobi777 · November 24, 2023, 4:57am

Sorry, I'm just a beginner. Can you tell me about non-blocking code?

kenobi777 · November 27, 2023, 2:26am

Hello. I've already tested SRF05 alone and it worked out well. However, I faced a problem again once I added it to the robot's operation.

Here is the code.

/*
 Example sketch for the PS4 Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */
#include <PS4BT.h>
#include <usbhub.h>

#include <math.h> 
#define PI 3.141592653589793

// #include <FlexiTimer2.h>
// #include <VarSpeedServo.h>

// VarSpeedServo myservo;
// int speed= 40;  

#define TRG 12
#define ECH 13
int loopdis;

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>

float measure(){
  int diff;
  float dis;

  digitalWrite(TRG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRG, LOW);
  diff = pulseIn(ECH, HIGH);
  dis = (float)diff * 0.01715;
  return dis;
}


USB Usb;
//USBHub Hub1(&Usb); // Some dongles have a hub inside
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so

/* You can create the instance of the PS4BT class in two ways */
// This will start an inquiry and then pair with the PS4 controller - you only have to do this once
// You will need to hold down the PS and Share button at the same time, the PS4 controller will then start to blink rapidly indicating that it is in pairing mode
PS4BT PS4(&Btd, PAIR);

// After that you can simply create the instance like so and then press the PS button on the device
//PS4BT PS4(&Btd);

bool printAngle, printTouch;
uint8_t oldL2Value, oldR2Value;



void setup() {
  Serial.begin(115200);

  pinMode(5,OUTPUT);   
  pinMode(4,OUTPUT);  
  pinMode(3,OUTPUT);   
  pinMode(2,OUTPUT);  

  // myservo.attach(8);

  pinMode(TRG, OUTPUT);
  pinMode(ECH, INPUT);

  pinMode(7,OUTPUT);

#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); // Halt
  }
  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
}




void loop() {
  Usb.Task();

  if (PS4.connected()) {
    int Lx;
    Lx= PS4.getAnalogHat(LeftHatX)-127;

    int Ly;
    Ly= 127-PS4.getAnalogHat(LeftHatY);

    double degree;
    degree= atan2(Ly,Lx)*180.0/PI+180;

    double radius;
    radius = sqrt(pow(Lx,2)+pow(Ly,2));


    loopdis= measure();
    if(loopdis<10){
      Serial.print(loopdis);
      Serial.println(" cm");
    }
    else{
      Serial.println("continue");
    }


    // if((radius>=45)&&(loopdis>=10))
    // {
    //   //*** forward ***//
    //   if ((degree>225)&&(degree<315)) 
    //   {
    //     Serial.print(F("\r\ndegree: "));
    //     Serial.print(degree);
    //     // Serial.println("go straight!");

    //     digitalWrite(5,HIGH);
    //     digitalWrite(4,LOW);
    //     digitalWrite(3,HIGH);
    //     digitalWrite(2,LOW);
    //   }

    //   //*** backward ***//
    //   if ((degree>45)&&(degree<135))
    //   {
    //     Serial.print(F("\r\ndegree: "));
    //     Serial.print(degree);
    //     // Serial.println("move backward");

    //     digitalWrite(5,LOW);
    //     digitalWrite(4,HIGH);
    //     digitalWrite(3,LOW);
    //     digitalWrite(2,HIGH);
    //   }

    //   //*** left ***//
    //   if (((degree>0)&&(degree<45))||((degree>315)&&(degree<360)))
    //   {
    //     Serial.print(F("\r\ndegree: "));
    //     Serial.print(degree);
    //     // Serial.println("turn LEFT!!!");

    //     digitalWrite(5,LOW);
    //     digitalWrite(4,HIGH);
    //     digitalWrite(3,HIGH);
    //     digitalWrite(2,LOW);
    //   }

    //   //*** right ***//
    //   if ((degree>135)&&(degree<225))
    //   {
    //     Serial.print(F("\r\ndegree: "));
    //     Serial.print(degree);
    //     // Serial.println("turn RIGHT!!!");

    //     digitalWrite(5,HIGH);
    //     digitalWrite(4,LOW);
    //     digitalWrite(3,LOW);
    //     digitalWrite(2,HIGH);
    //   }
    // }
    
    // //*** stop ***//
    // else if((radius<45)&&(loopdis>=10))
    // {
    //   Serial.println("STOP!");

    //   digitalWrite(5,LOW);
    //   digitalWrite(4,LOW);
    //   digitalWrite(3,LOW);
    //   digitalWrite(2,LOW);
    // }

    // //*** collision detection ***//
    // else if((radius>45)&&(loopdis<10))
    // {
    //   Serial.print(loopdis);
    //   Serial.println("collision");
    //   Serial.println("  cm");

    //   delay(2000);
    //   digitalWrite(5,LOW);      //move backward
    //   digitalWrite(4,HIGH);
    //   digitalWrite(3,LOW);
    //   digitalWrite(2,HIGH);
    // }

  }
}

I commented out all the movement for the robot and tested SRF05 part alone.

loopdis= measure();
    if(loopdis<10){
      Serial.print(loopdis);
      Serial.println(" cm");
    }
    else{
      Serial.println("continue");
    }

It supposed to show the distance from the obstacle in Serial Monitor, but it only showed 0 cm continuously.

xfpd · November 27, 2023, 2:49am

Show the value of diff... i think pulsein() is effecting the result.

kenobi777 · November 27, 2023, 3:04am

Sorry...I'm just a beginner. Can you tell me how to do it?

kenobi777 · November 27, 2023, 3:07am

When I comment out this part in setup()

// #if !defined(__MIPSEL__)
//   while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
// #endif
//   if (Usb.Init() == -1) {
//     Serial.print(F("\r\nOSC did not start"));
//     while (1); // Halt
//   }
//   Serial.print(F("\r\nPS4 Bluetooth Library Started"));

and put SRF05's operation before Usb.Task(); .It worked out normally on Serial Monitor.

Topic		Replies	Views
Servo - keine Bewegung Deutsch	113	784	January 25, 2026
Schleife mittels Tastendruck verlassen? Deutsch	57	362	September 29, 2025
Bluetooth Car bewegt sich nicht Deutsch	185	996	December 17, 2025
Ho un problema con i servo e con i led fade Software	10	331	July 16, 2024
Robot esapode con molti servi ; software Software	200	1704	June 13, 2025

Adding servo(SG90)

Related topics