Interacting Servo Robot coding Issue

Hi,

I have been working on a project that involves a robot moved by servos that interacts with the user. In order to achieve this the robot needs an input from the user and after processing it, provides an output.

To do this, I used the MPU-6050 6DOF inertial measurement unit and 2 standard metal geared servos (MG 966R). I have a section of code that deals specifically with managing all the required libraries and variables accessed all throughout. The part where the robot is actually moved by taking input or output is in the loop.

What I want to achieve is getting the person to push or touch the robot with a specific amount of force and have it move because of the input. My code consists of my own jammed together with Sweep from the examples folder and some tutorials on YouTube as I am a beginner. Here is the code for it in its most updated form, controlling two servos.

#include "Wire.h"    // Calls wire, I2Cdev, MPU, and Servo
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
MPU6050 mpu;

int16_t ax, ay, az;  //declares the required values
int16_t gx, gy, gz;

Servo myservo1;
Servo myservo2;
int pos = 0;    // variable to store the servo position
int backwardDelay = 15;
int forwardDelay = 15;
float step1 = 0;



void setup() {
  Wire.begin();
  Serial.begin(38400);
  Serial.println("Initialize MPU"); 
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "Ready" : "failed");
  myservo1.attach(9);
  myservo2.attach(10);
  }


 void callibrate(){
  for (pos = 0; pos <= 90; pos += 1){
    myservo1.write(pos);
    myservo2.write(pos);
    delay(15);
    }
 }

void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Records the raw data 
  float stepRec= (ax/5000);
  if (stepRec < 0){
    stepRec = 0;
    }
  if (stepRec != step1){
    step1 = stepRec;
    }
  Serial.println(step1);
  for (pos = 180; pos >= 55; pos -= step1) { // goes from 180 degrees to 55 degrees // in steps of given value
    myservo1.write(pos);              // tells servo to go to position in variable 'pos'
    delay(backwardDelay);                       // waits 15ms for the servo to reach the position
    }
  for (pos = 55; pos <= 180; pos += step1) { // goes from 55 degrees to 180 degrees
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(forwardDelay); // waits 15ms for the servo to reach the position
    }
  for (pos = 0; pos >= 125; pos -= step1) { // goes from 0 degrees to 125 degrees // in steps of given value
    myservo2.write(pos);              // tells servo to go to position in variable 'pos'
    delay(backwardDelay);                       // waits 15ms for the servo to reach the position
    }
  for (pos = 125; pos <= 0; pos += step1) { // goes from 125 degrees to 0 degrees
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
    delay(forwardDelay); // waits 15ms for the servo to reach the position
    }
    }

As you can see at the bottom the reason I used 125 and 0 is because the servos will be facing each other and I need them to move at the same direction which means one will be going clockwise and one anticlockwise. I have also programmed a calibrating function so that it sets it right at 90 degrees at the start to set it at the middle. While trying to investigate how I could make the servo move faster, I discovered that by manipulating the steps, you could get it to arrive at certain degrees in a desired amount of time based on the step.

I used both integers and floats and came to the conclusion that any value except negative values can be used for steps ( this was helpful later for my MPU). The moving of the servo worked perfectly fine until I combined it with the MPU’s code. What the MPU code does it that it retrieves raw data from the chip which is acceleration that I am using. I written a line of code that divides that by 5000 as I found it was a suitable number that would translate the raw data into a reasonable and proportionate step. Knowing that negative steps will cause issues, I written a line that would automatically set step to 0 if it is indeed smaller than 0 ( negative numbers). I also made sure to let the program set the recorded step as the step that will be used in the servo position commands just in case.

The problem right now seems to be that the loop is not working. I have placed serial.println at several parts to try and narrow down where it has gone wrong and have found that in the monitor, it displayed one MPU value of 0.00 then stopped with nothing else to say. To attempt to solve this issue I separated the code and tested the servo code and MPU code, which both worked just fine.

In hardware, I have provided an alternate power source for the servos using a 4 X 1.5V mini box of AA batteries, the USB powering Arduino, and the ground connected together. The MPU works just fine as I tested it with a i2cDev scanner and it displays values just fine with the servo code. As for the delays, I have set them to 15 as I found it quite reasonable and had no reason to mess with them as long as the steps worked. The serial monitor IS at 38400 baud rate as I tried 115200 which was the standard for Jeff’s Teaport ( a program for the MPU written by someone from MIT and the head of i2cDev), but found 38400 compatible for this specific program.

I would appreciate any help or reply as to where I have gone wrong and thank you for your reply and time.

Have you not already got a thread running on this subject ?

UKHeliBob: Have you not already got a thread running on this subject ?

After speaking to a more experienced member, we have came to the conclusion that it would be best if I create a new thread that details everything in a more elaborate manner for people to analyze. Thanks.

After speaking to a more experienced member, we have came to the conclusion that it would be best if I create a new thread that details everything in a more elaborate manner for people to analyze.

Actually, what I said was to update your existing thread...

As you can see at the bottom the reason I used 125 and 0 is because the servos will be facing each other and I need them to move at the same direction which means one will be going clockwise and one anticlockwise. I have also programmed a calibrating function so that it sets it right at 90 degrees at the start to set it at the middle. While trying to investigate how I could make the servo move faster, I discovered that by manipulating the steps, you could get it to arrive at certain degrees in a desired amount of time based on the step.

Actually, I can't understand what you are doing with the servos. The calibrate() function moves the servos to 0, then sweeps them to 90.

Then, in loop(), you have 4 loops to move the servos, but you only move them one at a time. While you have commented what each line does, which anyone can see, you have not commented on WHY you are moving the servos that way.

Yes, you can change the speed of the servo by making larger or smaller steps with a fixed delay. Or, you could take the a fixed number of steps with shorter or longer delays.

You need to comment on WHY you are moving the servos the way that you are.

The moving of the servo worked perfectly fine until I combined it with the MPU's code.

What is not obvious from looking at the code is why you are using the momentary rate of acceleration in the X direction to control the number of steps that the servos take. You need to explain why you think that is a reasonable value, and what the servos are doing.

The problem right now seems to be that the loop is not working.

Which loop? You have 4 for loops as well as the loop() function. Being specific in your explanations is critical.

I am trying to move both servos simultaneously as they can act like arms when I attach material to it.

I am uncertain about which loop is not repeating but I think something is preventing void loop from repeating.

I chose the x axis because I wanted it to interact when the person pushes it forward. The raw value comes out at around 10000 and below when a regular hand pushes it. by dividing by 5000 I get a reasonable step of 1-2. I want it to create a repetitive movement while being able to change the speed and hopefully can incorporate it to perform more tedious tasks.

The servo which moves clockwise goes to 180 and then goes to 55, I need the other servo moving anti-clockwise to do the same while facing it, so in a way it is kind of laterally inverted. I set it to 90 for calibrate because that will mean the arms rest straight down completely.

The reason I put "if" was to try and tailor it so that it could fit scenarios where negative numbers might arise. At the bottom two of the "for" are for the servo completely one movement then back to its position and the other two are for the other server moving simultaneously with it.

Maybe the issue derives from the fact that the servo itself contains too many "for"s? Should I look into finding another way to get the servo to move to positions without including for?

JLam:
Maybe the issue derives from the fact that the servo itself contains too many "for"s? Should I look into finding another way to get the servo to move to positions without including for?

If you indeed want the servos to move at the same time, you’ll have to cut out all that blocking code, so no while loops or for loops waiting for a servo to finish moving before moving the next one and absolutely no use of the delay function.

You can move both servos in one for loop:

   for(byte b=0; b<125; b++)
   {
       myServo1.write(b+55);
       myServo2.write(180-b);
   }

You can determine where the code stops by adding more, not less, Serial.print() statements.

Thank you all for your inputs, I will implement these suggestions and record the outcome and update here once I'm done with school. Thanks a lot.

Just to update I have condensed those loops into this:

for (byte v =90; v<125; v+= 1){
  myservo1.write(v-35);             
  myservo2.write(v+35);              
  delay(backwardDelay);                       
    }
 for(byte v = 90; v<125; v+= 1) {
  myservo1.write(90);              
  myservo2.write(90);              
  delay(forwardDelay);

This SHOULD move the two servos directly facing each other to the angles I want at a desired step. By the way I actually run this with a servo earlier on and it displayed 5 raw values instead of just 1 so its getting somewhere I think.

for(byte v = 90; v<125; v+= 1) {
  myservo1.write(90);              
  myservo2.write(90);              
  delay(forwardDelay);

What’s the point of repeatedly writing 90 to the servo over and over? If you write it once then then servo should go there.

Delta_G:

for(byte v = 90; v<125; v+= 1) {

myservo1.write(90);             
  myservo2.write(90);             
  delay(forwardDelay);




What's the point of repeatedly writing 90 to the servo over and over? If you write it once then then servo should go there.

I find that when I do not put the 90 degree loop at the very end it does not go all the way there. Right now theres a weird issue where one of the servos move before the other and I’m trying to solve that too. It moves in the same direction but the other one is like half a second off

Post the complete updated code.

#include "Wire.h"    // Calls wire, I2Cdev, MPU, and Servo
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
MPU6050 mpu;

int16_t ax, ay, az;  //declares the required values
int16_t gx, gy, gz;

Servo myservo1;
Servo myservo2;
int pos = 0;    // variable to store the servo position
int backwardDelay = 15;
int forwardDelay = 15;
float step1 = 0;



void setup() {
  Wire.begin();
  Serial.begin(38400);
  Serial.println("Initialize MPU"); 
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "Ready" : "failed");
  myservo1.attach(9);
  myservo2.attach(10);
  }


 void callibrate(){
  for (pos = 0; pos <= 90; pos += 1){
    myservo1.write(pos);
    myservo2.write(pos);
    delay(15);
    }
 }

void loop() {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Records the raw data 
  float stepRec= (ax/5000);
  if (stepRec <= 0){
    stepRec = 0;
    }
   
    Serial.println(step1);
    for (byte v =90; v<125; v+= stepRec){
      myservo1.write(v-45);              // tells servo to go to position in variable 'pos'
      myservo2.write(v+35);
      delay(15);// tells servo to go to position in variable 'pos'
      }
    for (byte v =90; v<125; v+= stepRec){
      myservo1.write(100);              // tells servo to go to position in variable 'pos'
      myservo2.write(90);
      delay(15);// tells servo to go to position in variable 'pos'
      }
      
}

I apologize for the changes in comments and stuff as I am constantly trying to think of ways to make it better and even wanna make it so that it stops if MPU value = 0, but that’s not the priority. I’m trying to get this to work. If you can spot any errors then that would be brilliant. I really appreciate your help btw, thanks. [/code]

#include "Wire.h"    // Calls wire, I2Cdev, MPU, and Servo

Including header files does NOT call anything.

int16_t ax, ay, az;  //declares the required values

Isn’t that obvious?

Comments should define WHY you are doing something, not what the code is doing. In general, the what is perfectly obvious.

Servo myservo1;
Servo myservo2;
int pos = 0;    // variable to store the servo position

Of which servo?

 void callibrate(){
  for (pos = 0; pos <= 90; pos += 1){
    myservo1.write(pos);
    myservo2.write(pos);
    delay(15);
    }
 }

That is NOT calibrating anything. That is slowly rotating the servo from where it may not be to 90. Change the name of the function!

    for (byte v =90; v<125; v+= stepRec){
      myservo1.write(100);              // tells servo to go to position in variable 'pos'
      myservo2.write(90);
      delay(15);// tells servo to go to position in variable 'pos'
      }

It makes no sense to tell the servo to go to the same place over and over. Either it gets there as a result of being told once, or it won’t ever get there, no matter how many times you tell it to move.

PaulS:

 void callibrate(){

for (pos = 0; pos <= 90; pos += 1){
    myservo1.write(pos);
    myservo2.write(pos);
    delay(15);
    }
}



That is NOT calibrating anything. That is slowly rotating the servo from where it may not be to 90. Change the name of the function!



for (byte v =90; v<125; v+= stepRec){
      myservo1.write(100);              // tells servo to go to position in variable ‘pos’
      myservo2.write(90);
      delay(15);// tells servo to go to position in variable ‘pos’
      }



It makes no sense to tell the servo to go to the same place over and over. Either it gets there as a result of being told once, or it won't ever get there, no matter how many times you tell it to move.

Hi thanks for the comment. I named it calibrate because I wanted to make sure that at the start of the process it first of all adjusts the angle to 90 degrees in this case facing down. I’m doing this because I decided to attach my servo in a vertical way (Sorry for not elaborating on that) to save some space.

As for the telling it to go to 90, I just wanted to make sure it goes to 90 after it goes to the desired angle at a desired speed in this case 125 for the anticlockwise servo and 55 for the clockwise servo as (+35) and (-35). If I don’t do it then it will just stay at that angle I told it to go to in the first place above. I made one 100 and one 90 because as of now one servo moves slower than another and I found adjusting it to a 100 makes it move at a more “similar speed”. I discovered the terror that servos are not the same most of times so I am taking that into account. I am trying to solve the current problem at hand where it only loops 6-7 times ( An improvement!). Now I just need to make that loop forever and I think something can be done to make that work, I just can’t see it at the moment.

Edit: Id like to add that an interesting thing I discovered is that the amount of values output in the serial monitor seems to depend on what I use. For example when I use “ay” it only gives 0 where it should be giving more than that, but when I use “ax” it gives about 6 values then just stops receiving.

Second edit: I think I KNOW. In the code I tell the servo to go to these points, but I don’t even have the servo hooked up when I try the MPU code. There servo code is still there so there reason it stops might well be because the stuff its trying to send to the servo is set as “not received” therefore stopping it. Not sure if theres some transmission manager inside the Arduino but it might well be, trying it out

Nvm still freezing after some values.

Edit: I am now convinced that this is a power issue..

Let me elaborate on my connections then:

  1. I connected the two metal gear servos to the Arduino as well as the MPU 6050 (mistake)

  2. I connected two metal gear servos to a 4 X 1.6v box and connected the yellow wire as well as the ground to Arduino. The MPU still fully connected to the Arduino

  3. I did the same thing above but this time I connected the MPU 6050 ground to the breadboard and had the voltage attached to the servo's battery back with a 220 ohm resister between.

Any suggestions? I read somewhere that the SDA and SCL lines might need resistors too but our scenarios seem different. By the way just to confirm my values now freeze intermittently, so they work for a bit then just stop. I also confirmed that the MPU 6050 is NOT broken as I tested it with other MPU codes such as Jeff Rowberg's stuff.

Update March 20th: Used the highly suggested SDA and SCL 10k ohm resistors during the test and while it seems to be quite efficient with other MPU code, still does not loop in this specific one. For now I am writing a backup for the robot using an IR remote which I loathe but if you do spot an error please do alert me. I have also ruled out dealing with negative values and made it so that the step uses all values calculated.