Hello and Happy new Year to all!
I am trying to build an arm with the self-balance function using the MPU6050 Gyro and an Arduino Uno. While writing the sketch, i ran into a problem regarding the while loops, actually I cannot exit the loop once it starts to execute. And i can't seem to figure out what I missed.
Regardless how I move the gyro, eaven when the position is reversed, there is no change on the serial monitor.
Also search on the forum for clues and same story. Bellow is the sketch and a prinscreen of the serial monitor. Any help is highly appreciated.
Thank you all!
L.E. - I updated the code, fix syntax errors, same story.
#include <Wire.h>
const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;
int xdeg; //angle to be converted from 360 degrees to +/-180 degrees on X axys
int ydeg; //angle to be converted from 360 degrees to +/-180 degrees on Y axys
double offsetp = 20; //positive angular tolerance in respect to the horizontal line
double offsetm = -20; //negative angular tolerance in respect to the horizontal line
void setup(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}
void loop(){
boolean validatorx1;
boolean validatorx2;
boolean xalign = false;
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
// convert the X axis angles from 360 degrees to +/- 180 degrees format
if (x < 180) {
xdeg = x;
}
if (x > 180) {
xdeg = 180 - 1;
}
// convert the Y axis angles from 360 degrees to +/- 180 degrees format
if (y < 180 ){
ydeg = y;
}
if (y > 180 ) {
ydeg = 180 - y;
}
Serial.print("X Angle = ");
Serial.println(xdeg);
Serial.print("Y Angle= ");
Serial.println(ydeg);
Serial.println("-----------------------------------------");
if (xdeg > offsetp) { //check the positive deviation on X axis
validatorx1 == false;
}
if (xdeg < offsetm) { //check the negative deviation on Y axis
validatorx2 == false;
}
while (validatorx1 == false) { //if on the X axis the deviation is positive then engage one servo motor in order to lift then plate until
Serial.println ("Lifting Right"); // the angle is between 0 and offsetp value
Serial.println ("Angle difference is "); // here it starts the loop, but regardless how I rotate the MPU6050, eaven if the conditions to exit the loop
Serial.print (xdeg-offsetp); // are fulfilled, it never does.
if (xdeg < offsetp) {
break;
}
}
while (validatorx2 == false) { //if on the Y axis the deviation is positive then engage another servo motor in order to lift then plate until
Serial.println ("Lifting Left"); // the angle is between 0 and offsetp value
Serial.println ("Angle difference is "); // here it starts the loop, but regardless how I rotate the MPU6050, eaven if the conditions to exit the loop
Serial.print (offsetm + xdeg); // are fulfilled, it never does.
delay (1000);
validatorx2 == true;
}
if ((validatorx1) and (validatorx2)) { //after the plate is alligned to X axis, the alignment moves to y axis
Serial.println ("X axis is alligned - moving to Y axis"); //never reach to test this loop because of the other two that are not exiting
xalign == true;
}
delay (1000);
}
while (validatorx1 == false) //if on the X axis the deviation is positive then engage one servo motor in order to lift then plate until
{
Serial.println ("Lifting Right"); // the angle is between 0 and offsetp value
Serial.println ("Angle difference is "); // here it starts the loop, but regardless how I rotate the MPU6050, eaven if the conditions to exit the loop
Serial.print (xdeg - offsetp); // are fulfilled, it never does.
if (xdeg < offsetp)
{
break;
}
}
It loops forever if validatorx1 is false, which it is - there is nothing anywhere in your code to set it true.
You can break the loop if xdeg < offsetp, but apparently it isn't, so it loops forever, as you observe.
It appears from the comments that you are expecting xdeg to change during the while loop. It won't. xdeg is set earlier in your loop function but that code won't be executed again until your while loop exits, which will never happen.
You have a (common) fundamental misunderstanding of how and when variables change in code.
// minimal MPU-6050 tilt and roll (sjr)
// works perfectly with GY-521, pitch and roll signs agree with arrows on sensor module 7/2019
// tested with eBay Pro Mini, **no external pullups on SDA and SCL** (works with internal pullups)
// A4 = SDA, A5 = SCL
#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;
void setup() {
Wire.begin(); //begin the wire communication
Wire.beginTransmission(MPU_addr1); //begin, send the slave adress (in this case 68)
Wire.write(0x6B); //make the reset (place a 0 into the 6B register)
Wire.write(0);
Wire.endTransmission(true); //end the transmission
Serial.begin(9600);
}
void loop() {
Wire.beginTransmission(MPU_addr1);
Wire.write(0x3B); //send starting register address, accelerometer high byte
Wire.endTransmission(false); //restart for read
Wire.requestFrom(MPU_addr1, 6, true); //get six bytes accelerometer data
int t = Wire.read();
xa = (t << 8) | Wire.read();
t = Wire.read();
ya = (t << 8) | Wire.read();
t = Wire.read();
za = (t << 8) | Wire.read();
// formula from https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
roll = atan2(ya , za) * 180.0 / PI;
pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied
Serial.print("roll = ");
Serial.print(roll,1);
Serial.print(", pitch = ");
Serial.println(pitch,1);
delay(400);
}
while (validatorx1 == false) //if on the X axis the deviation is positive then engage one servo motor in order to lift then plate until
{
Serial.println ("Lifting Right"); // the angle is between 0 and offsetp value
Serial.println ("Angle difference is "); // here it starts the loop, but regardless how I rotate the MPU6050, eaven if the conditions to exit the loop
Serial.print (xdeg - offsetp); // are fulfilled, it never does.
if (xdeg < offsetp)
{
break;
}
}
It loops forever if validatorx1 is false, which it is - there is nothing anywhere in your code to set it true.
You can break the loop if xdeg < offsetp, but apparently it isn't, so it loops forever, as you observe.
It appears from the comments that you are expecting xdeg to change during the while loop. It won't. xdeg is set earlier in your loop function but that code won't be executed again until your while loop exits, which will never happen.
You have a (common) fundamental misunderstanding of how and when variables change in code.
The condition xdeg<offsetp is achieved by tilting the sensor by hand for the moment. Anyway, I tilted the sensor way more than necessary to achieve the condition, but the while loop continued displaying same values over and over again. Also the same condition should turn the value of validatorx1 to true
You're missing the point. You have a bunch of code at the start of the loop function that sets xdeg based on what you read from your MPU.
Having read it, you run your while loop. There is nothing in your while loop that changes xdeg, so if it doesn't exit on the first iteration, it never will. You can wave the MPU around as much as you like, but nothing is reading it.
So the problem was solved. I needed to rewrite the while loops, to work as intented. Also I missed to init the MPU6050 inside every while loop. Just as bonus, I re-wrote the entire scetch using the functions. Now the while loops look like this:
void balance_right() {
while ((varx1 == 0) && (varx2 == 1)) { //these are established in a previous validation function.
mpu.Execute(); //this is the sensor init
Serial.println ("Lifting right cylinder");
if (angx <= refn) { //checking the conditions
angx = mpu.GetAngX();
Serial.println ("Actual X angle");
Serial.println (angx);
}
else if (angx > refn) {
Serial.println ("Condition fulfilled");
Serial.println (angx);
angx = 0;
}
}
}
