Servos don't get controlled properly

Hi all,

First of all thanks for the amazing work everyone is doing in helping here.

I am very new to arduino and I am trying to make a 3 axis skull that can talk. I have read (and tried) a lot already but still the servo’s are only twitching a bit. every thing is hooked up to a 5VDC 4A powersupply.
In where the servo’s are sepperated from the arduino and GND are connected together.

Some of the things that I have tried or read trough are;

If I only try the “test software” from “Several Things at a Time” (tried only with one servo) the servo is working perfect but when i incorporate the software from “Several Things at a Time” in my own program I get twitching servo.

I will attach my program (it is to big to put it in a code block. Sorry).

I hope someone will be able to make sense of things.

Sorry for my bad english it is not my native language.

with regards Marcel,

Schedel_Karel_V09.ino (15.4 KB)

Hi all,

I got rid of allmost all the Delay(), but still twitching servo’s.

I hope someone can help me.

Also posted in the Netherlands section of the forum.

Please post a schematic.

Hope you did not make the dumb mistake of connecting power to “Vin” or “RAW”?

I don’t want to be a downer but your code is a mess. It seems to have been Frankenstein-ed together with bits from multiple sources. It screams to be cleaved into multiple sources to reduce clutter and ease readability.

Perhaps you need to get back to a basic functional sketch and then carefully add things one at a time.

I tried to re-do some of your basic head servo code without the audio, distance trigger (although the ping code is there) or jaw code. You can try this to establish basic functionality of the head servo operation:

#include "Arduino.h"
#include <Servo.h>

#define NUM_SERVOS      3

//************************FOR PING SENSOR***************************

const byte trgPin = 7;     // pin we're sending the trigger/ping signal on
const byte echoPin = 8;    // pin we're reading back the echo on
const long motionDist = 15; // distance that triggers a sound (5 for testing)
//
long distance = 0;       // distance to be measured/recorded by ping sensor

//************************FOR SERVOS***************************

// create a servo object
Servo 
    //ServoJaw,
    ServoShake, 
    ServoNod, 
    ServoTilt;
    

typedef struct structServo
{
    Servo           *ptrServo;
    unsigned long   ulSlowInterval;
    unsigned long   ulFastInterval;
    unsigned long   ulCurrInterval;
    int             nDegrees;
    unsigned long   timeServo;
    int             nCurrPosition;
    int             nMinPosition;
    int             nMaxPosition;
    
}structServo;

structServo ServoData[NUM_SERVOS] =
{
    {
        .ptrServo = (Servo *)&ServoTilt,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 55,
        .nMinPosition = 10,
        .nMaxPosition = 100
    },
    {
        .ptrServo = (Servo *)&ServoNod,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 105,
        .nMinPosition = 70,
        .nMaxPosition = 140
    },
    {
        .ptrServo = (Servo *)&ServoShake,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 75,
        .nMinPosition = 10,
        .nMaxPosition = 130
    }
    
};

//set the first parameter in the following functions to a number between 0 and 180.
//int ServoJawMin = (map(92, 0, 180, 544, 2400)); //???
//int ServoJawMax = (map(72, 0, 180, 544, 2400)); //???

void setup( void )
{
    // Set Up TRIGGER SENSOR
    pinMode(trgPin, OUTPUT);
    digitalWrite(trgPin, LOW);
    pinMode(echoPin, OUTPUT);    // just to make sure
    digitalWrite(echoPin, LOW);  // we clear any previous settings
    pinMode(echoPin, INPUT);     // and then use it as INPUT
    
    Serial.begin(115200);
    Serial.println("initializing...");
    
    //  Set up DFPlayer
    //InitAudio();

    attach_servos();
    
}//setup

void loop()
{
    Headservos( millis() );
}

void attach_servos()
{ 
    // attach the pin to the servo object
    //ServoJaw.attach(2, ServoJawMin, ServoJawMax);
    ServoShake.attach(3);
    ServoTilt.attach(5);
    ServoNod.attach(6);

    //servosEnabled = true;
    //Serial.print ("servo's are enabled");
}

void detach_servos()
{ 
    // detach the servo objects
    //ServoJaw.detach();
    ServoShake.detach();
    ServoTilt.detach();
    ServoNod.detach();

    //servosEnabled = false;
    //Serial.print ("servo's are disabled");
}

void Headservos( unsigned long tNow )
{
    for( int srvo=0; srvo<NUM_SERVOS; srvo++ )
    {
        if( tNow - ServoData[srvo].timeServo >= ServoData[srvo].ulCurrInterval )
        {
            ServoData[srvo].timeServo = tNow;
            
            ServoData[srvo].nCurrPosition += ServoData[srvo].nDegrees;
            
            if( ServoData[srvo].nCurrPosition >= ServoData[srvo].nMaxPosition ||
                    ServoData[srvo].nCurrPosition <= ServoData[srvo].nMinPosition )
            {
                ServoData[srvo].nDegrees *= -1;
            }

            ServoData[srvo].ptrServo->write( ServoData[srvo].nCurrPosition );
            
        }//if
        
    }//for

    //ServoJaw.write(audio_value);
    
}//Headservos

long getDist()
{
    // establish variables for duration of the ping,
    // and the distance result in inches and centimeters:
    long 
        duration, 
        inches;

    digitalWrite(trgPin, HIGH);      // start the outgoing ping
    delayMicroseconds(10);            // do the ping for 10uS
    digitalWrite(trgPin, LOW);       // stop doing the ping
    duration = pulseIn(echoPin, HIGH);  // grab the delay of return echo
    
    return( duration / 29 / 2 );
    
}//getDist

Paul__B:
Hope you did not make the dumb mistake of connecting power to "Vin" or "RAW"?

I gues I am that dumb. I read in a couple of specifications that Vin Is power supply 6-20 volts.
I am sure that you can tell me what I did wrong. And how I am suppose to read specifications.

Blackfin:
I don't want to be a downer but your code is a mess. It seems to have been Frankenstein-ed together with bits from multiple sources. It screams to be cleaved into multiple sources to reduce clutter and ease readability.

Perhaps you need to get back to a basic functional sketch and then carefully add things one at a time.

You are quite wright. So you are not a downer. I thought that I could make it work if I where to combine different sketches.
But the dfplayer makes it quite hard to code everything.

Ik tried your skech and it is working like a charm. I am going to try to work from there and try and sort everything out. I will keep you posted.

with regards Marcel

Sunny1973:
I gues I am that dumb. I read in a couple of specifications that Vin Is power supply 6-20 volts.
I am sure that you can tell me what I did wrong. And how I am suppose to read specifications.

A 5V power supply is not in the range 6-20 volts. When you have a 5V supply it should be connected to the 5V pin. It works much better that way.

Steve

Hi,

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png?

Have you got code that JUST sweeps the three servos, nothing else?

Thanks… Tom… :slight_smile:

slipstick:
A 5V power supply is not in the range 6-20 volts. When you have a 5V supply it should be connected to the 5V pin. It works much better that way.

Steve

pff now I see it I was overlooking it. Oke you are right that was dumb. :slight_smile:

Sorry Paul B. you where verry right.

I have drawn a quick scematic. At the lose ends of wires there is an other module that I could not find in fritzing.

How to connect that module is shown in:

http://buttonbanger.com/downloads/jawduino/jawduino.pdf

My complete code is in post 1. But as Blackfin posted that is al mess. So now I am trying to clean that up.

with regards Marcel

Hi Blackfin

I have tried your code and that worked nicely. So now I am trying to understand it.

void Headservos( unsigned long tNow )
{
    for( int srvo=0; srvo<NUM_SERVOS; srvo++ )
    {
        if( tNow - ServoData[srvo].timeServo >= ServoData[srvo].ulCurrInterval )
        {
            ServoData[srvo].timeServo = tNow;
           
            ServoData[srvo].nCurrPosition += ServoData[srvo].nDegrees;
           
            if( ServoData[srvo].nCurrPosition >= ServoData[srvo].nMaxPosition ||
                    ServoData[srvo].nCurrPosition <= ServoData[srvo].nMinPosition )
            {
                ServoData[srvo].nDegrees *= -1;
            }

            ServoData[srvo].ptrServo->write( ServoData[srvo].nCurrPosition );
           
        }//if
       
    }//for

    //ServoJaw.write(audio_value);
   
}//Headservos

I tried to incorperate the 4th servo the jaw servo. If I let it go with the other three it goes aswell so that is nice. but now I would like to let that servo move with the sound that.
And if I try to put the code in for the DFPlayer I get jittering servo’s again. I gues that I do something wrong with calling the code for the DFPlayer but I can’t see what I am doing wrong.
I hope that someone can help me.

Sunny1973:
Hi Blackfin

I have tried your code and that worked nicely. So now I am trying to understand it.

void Headservos( unsigned long tNow )

{
    for( int srvo=0; srvo<NUM_SERVOS; srvo++ )
    {
        if( tNow - ServoData[srvo].timeServo >= ServoData[srvo].ulCurrInterval )
        {
            ServoData[srvo].timeServo = tNow;
         
            ServoData[srvo].nCurrPosition += ServoData[srvo].nDegrees;
         
            if( ServoData[srvo].nCurrPosition >= ServoData[srvo].nMaxPosition ||
                    ServoData[srvo].nCurrPosition <= ServoData[srvo].nMinPosition )
            {
                ServoData[srvo].nDegrees *= -1;
            }

ServoData[srvo].ptrServo->write( ServoData[srvo].nCurrPosition );
         
        }//if
     
    }//for

//ServoJaw.write(audio_value);
 
}//Headservos

I purposely left the jaw out because I saw that you had wanted to control it differently (i.e. servo position a function of incoming audio vs just following a pattern.)

For the servos all doing repetitive actions I just encapsulated their properties into a structure and made an array of three of these, one for each of the repetitive servos.

It looks complicated but basically follows the logic of your original code; move a servo one degree every time interval until a limit is hit, then reverse direction until the limit is hit etc.

Before jumping all the way to the audio, how about adding sensing the ultrasound distance sensing? Having the servos stay quiet until something gets within a specified distance, then start the servos?

Can you better describe how the audio works? As I said before, it appears you’ve copy-pasted DFPlayer code from somewhere else but it may not be right for your project. What, exactly, do you want it to do? How many audio files are there? How does the audio-sensing circuit and logic work?

I thought as much that you left the jaw servo out for that reason.
I have added the PING sensor and it looks like that is working.
If I go close to the sensor the servos start moving.
The onlything is and that was also befor adding the ping sensor the sensors move realy slow. I gues that is because of the value of “ulCurrInterval”. so if I like I can speed dat up a bit.

For what I would like to program.

I would like to have a playlist of songs that are on the DFPlayer to play one by one. If someone would come (to) near I like to play a sound from a second playlist.
There is a function on the DFPlayer that is called advertise. If I call that than the song is pauzed and the advertisment is played.

DFPlayer_Mini_SKU_DFR0299-DFRobot this is a some info on the player.

I wil attach the code as well. for if you would like a look.

#include "Arduino.h"
#include <Servo.h>

#define NUM_SERVOS      3

//************************FOR PING SENSOR***************************

const byte trgPin = 7;     // pin we're sending the trigger/ping signal on
const byte echoPin = 8;    // pin we're reading back the echo on
const long motionDist = 15; // distance that triggers a sound (15 for testing)
//const   long motionDist=35; // distance that triggers a sound (29-40 for real life)
long distance = 0;       // distance to be measured/recorded by ping sensor


//************************FOR SERVOS***************************

                // This is for the Jaw//
                
unsigned long   JawSlowInterval = 80;
unsigned long   JawFastInterval = 10;
unsigned long   JawCurrInterval = 80;
int             JawDegrees = 1;
unsigned long   JawtimeServo = 0;
int             JawCurrPosition = 40;
int             JawMinPosition = 10;
int             JawMaxPosition = 65;

// create a servo object

Servo
    ServoJaw,
    ServoShake,
    ServoNod,
    ServoTilt;
   

typedef struct structServo
{
    Servo           *ptrServo;
    unsigned long   ulSlowInterval;
    unsigned long   ulFastInterval;
    unsigned long   ulCurrInterval;
    int             nDegrees;
    unsigned long   timeServo;
    int             nCurrPosition;
    int             nMinPosition;
    int             nMaxPosition;
   
}structServo;

structServo ServoData[NUM_SERVOS] =
{
    {
        .ptrServo = (Servo *)&ServoTilt,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 55,
        .nMinPosition = 10,
        .nMaxPosition = 100
    },
    {
        .ptrServo = (Servo *)&ServoNod,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 105,
        .nMinPosition = 70,
        .nMaxPosition = 140
    },
    {
        .ptrServo = (Servo *)&ServoShake,
        .ulSlowInterval = 80,
        .ulFastInterval = 10,
        .ulCurrInterval = 80,
        .nDegrees = 1,
        .timeServo = 0,
        .nCurrPosition = 75,
        .nMinPosition = 10,
        .nMaxPosition = 130
    },
    
   
};


//************************FOR AUDIO***************************

int audio_value = 0;                      //stores the calculated audio value
long lastMsg = 0;                         //stores millis
long sleepWindow = 300000;                //if 5 minutes go by with no signal, then put the servos to bed
volatile boolean servosEnabled = false;   //check if servos are enabled
volatile boolean ledsOn = true;           //for turning on the leds of the eyes
volatile unsigned long currentTime = 0;
volatile unsigned long lastLEDtime = 0;
unsigned long resetWait = 120000;         //servos sleep if not changed within this time frame (120 secs)


//set the first parameter in the following functions to a number between 0 and 180.
//int ServoJawMin = (map(92, 0, 180, 544, 2400)); //???
//int ServoJawMax = (map(72, 0, 180, 544, 2400)); //???

void setup( void )
{
    // Set Up TRIGGER SENSOR
    pinMode(trgPin, OUTPUT);
    digitalWrite(trgPin, LOW);
    pinMode(echoPin, OUTPUT);    // just to make sure
    digitalWrite(echoPin, LOW);  // we clear any previous settings
    pinMode(echoPin, INPUT);     // and then use it as INPUT
   
    Serial.begin(115200);
    Serial.println("initializing...");
   
    //  Set up DFPlayer
    //InitAudio();

    attach_servos();
   
}//setup

void loop()
{
    distance = getDist();
    delay(2);
    Serial.print("distance = ");
    Serial.println(distance);
  
    if (distance <= 5)
    {
    Headservos( millis() );
    }
    
}

void attach_servos()
{
    // attach the pin to the servo object
    ServoJaw.attach(2);
    ServoShake.attach(3);
    ServoTilt.attach(5);
    ServoNod.attach(6);

    //servosEnabled = true;
    //Serial.print ("servo's are enabled");
}

void detach_servos()
{
    // detach the servo objects
    ServoJaw.detach();
    ServoShake.detach();
    ServoTilt.detach();
    ServoNod.detach();

    //servosEnabled = false;
    //Serial.print ("servo's are disabled");
}


void action( unsigned long Jawmil)
{ 
  /*
if (Jawmil - JawtimeServo >= JawFastInterval)
{
  JawtimeServo = Jawmil;

  JawCurrPosition += JawDegrees;

  if (JawCurrPosition >= audio_value || JawCurrPosition <= audio_value)
  {
    JawCurrPosition = - JawDegrees;
  }
  ServoJaw.write(JawCurrPosition);
}
  */
}

void Headservos( unsigned long tNow )
{
    for( int srvo=0; srvo<NUM_SERVOS; srvo++ )
    {
        if( tNow - ServoData[srvo].timeServo >= ServoData[srvo].ulCurrInterval )
        {
            ServoData[srvo].timeServo = tNow;
           
            ServoData[srvo].nCurrPosition += ServoData[srvo].nDegrees;
           
            if( ServoData[srvo].nCurrPosition >= ServoData[srvo].nMaxPosition ||
                    ServoData[srvo].nCurrPosition <= ServoData[srvo].nMinPosition )
            {
                ServoData[srvo].nDegrees *= -1;
            }

            ServoData[srvo].ptrServo->write( ServoData[srvo].nCurrPosition );
           
        }//if
        
       
    }//for

    //ServoJaw.write(audio_value);
   
}//Headservos

long getDist()
{
  long duration, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trgPin, HIGH);      // start the outgoing ping
  delayMicroseconds(10);            // do the ping for 10uS
  digitalWrite(trgPin, LOW);       // stop doing the ping
  duration = pulseIn(echoPin, HIGH);  // grab the delay of return echo
 
  cm = microsecondsToCentimeters(duration);
 
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

  return (cm);
}

// Original code from the ping sensor library
long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

It looks like that if I add "audio_updates" (that is where I measure three analoge values from the VU meter) everything stops.

And I don't have a clou how to solve that. Because I need a quick value for the Jawservo and I suppose I have to keep measuring the distance as well.
Is it possible that 1 ping sensor and 3 analoge values is to much for the nano to keep the jaw servo fast?

If I disable the PING sensor and add the "audio_updates" the three servos are working again. And the other way arround also.
Although it looks like with added PING sensor that the servos are more irregular.

I fear you're just copy-pasting stuff back in without "designing" the code and are breaking the logic in doing so.

I think you need to be using a state machine to control this and your code shouldn't have a single delay() (outside, perhaps, those used for the ultrasonic ping) or blocking code (e.g. while( ... ).)

that is wat I was trying to do. I don't have a clou hou to make a state machine. can you help me with that? For now it is getting late in the netherlands. So I am going to try tomorrow.
Thanks for all the help.

When you get a chance, download the attached zip and extract the contents to a new folder. It contains an ino file as well as separate audio.cpp and audio.h files.

They compile here but I don’t know if they function or how close (or far) they are from what you want.

It’s a starting point, anyway.

The ino file is called sketch_oct26e.ino so you probably want to copy them into a folder called “sketch_oct26e” to follow the IDE’s naming requirements.

sketch_oct26e.zip (3.4 KB)

First of all thank you verry much for the sketch you send me.

I am testing with the sketch. Butit wil not run. It looks like it keeps going to "ST_YAPPING".
And I noticed that you put the jawservo also in the struct but the "NUM_SERVOS" was still 3. Is that for a reason?

Also can you mabye explain to me why you put sound in a different liibrary. Does that make it faster?

I am going to test some more.

Again thanks for helping.

Sunny1973:
First of all thank you verry much for the sketch you send me.

I am testing with the sketch. Butit wil not run. It looks like it keeps going to “ST_YAPPING”.
And I noticed that you put the jawservo also in the struct but the “NUM_SERVOS” was still 3. Is that for a reason?

Also can you mabye explain to me why you put sound in a different liibrary. Does that make it faster?

I am going to test some more.

Again thanks for helping.

After a quick look, it appears I missed a “break” statement:

.
.
.
            else
            {
                spinAudio( AUDIO_PLAY );

            }//else

        break;    //<<<<<<< add this break statement

        case    ST_YAPPING:            
            if( CheckTrigger() >= RELEASE_DIST )
.
.
.

Sorry, I wrote it while watching The Terminator so I may have erred in other places too.

I moved the audio to another file to streamline the code.

Putting everything in a single function is bad form. Adding more and more functions to a single .ino or .cpp file is also bad. It would be good, for example, to have another cpp file for the servo stuff.

Separating things out makes it clearer and easier to debug.