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.