Hi everyone!!
I am using multiple sensors(MPU6050) using I2C protocol which works only on 2 addresses. So I am trying to run a dynamic code in which each sensor is assigned to anyone of 2 addresses
The problem here is I want to call loop() from setup() each time a sensor is being read. But once the loop() initiates it doesn't returning to for loop in setup().
Any advice is accepted and appreciated. Thanks
Maruthi_Garnepudi:
Hi everyone!!
I am using multiple sensors(MPU6050) using I2C protocol which works only on 2 addresses. So I am trying to run a dynamic code in which each sensor is assigned to anyone of 2 addressesThe problem here is I want to call loop() from setup() each time a sensor is being read. But once the loop() initiates it doesn't returning to for loop in setup().
Any advice is accepted and appreciated. Thanks
This seems very unusual. Can you show the code you are working with?
All depend on your code. It should work, but the call of loop() can be optimized out by compiler due the settings.
once the loop() initiates it doesn't returning to for loop in setup().
Have you got an infinite loop in setup() to prevent it falling through to loop() ? If not then once setup() has run loop() will execute and the hidden main() function will call it again repeatedly which would give the effect you describe.
Having said that, I can't imagine why you would want to call loop() from setup(). Why exactly do you want to do it ?
Maruthi_Garnepudi:
I am using multiple sensors(MPU6050) using I2C protocol which works only on 2 addresses. So I am trying to run a dynamic code in which each sensor is assigned to anyone of 2 addresses
XY problem?
Seems you want to use more than two sensors,
and you don't know that you can use one address pin as chip-select pin.
Leo..
Maruthi_Garnepudi:
The problem here is I want to call loop() from setup() each time a sensor is being read.
This is silly.
Don't read the sensors in setup(). Just read them in loop() and then they can be repeated as often as you like.
...R
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* Stepper*/
double sr=0,stt=0,stt1=0,cnt=0,m=0, timeRotate=0;
int count=0;
int RESET=13;
int SLEEP=12;
int STEP=11;
int DIR=10;
int HalfStep=9;
String s="";
int pulse(int interval=1);
/********************************/
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw,rep=0;
int k=0;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
int n=3,i;
int pin[3]={32,34,36};
// TODO: Make calibration routine
void setup() {
/********Stepper Setup**********/
pinMode(RESET,OUTPUT);
pinMode(SLEEP,OUTPUT);
pinMode(STEP,OUTPUT);
pinMode(DIR,OUTPUT);
pinMode(HalfStep,OUTPUT);
digitalWrite(DIR,LOW);
digitalWrite(RESET,HIGH);
digitalWrite(SLEEP,HIGH);
digitalWrite(HalfStep,HIGH);
/*********************************/
pinMode(pin[0],OUTPUT);
pinMode(pin[1],OUTPUT);
pinMode(pin[2],OUTPUT);
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
for(i=1;i<=n;i++){digitalWrite(pin[i],HIGH);}
i=1;
while(i<=n){digitalWrite(pin[i],LOW);
digitalWrite(pin[i-1],HIGH);
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
sr=0;
// delay(5000);
stt=millis();
loop();
}
}
void loop() {
k=1;
IMU();
if (k==2){Serial.println(kalAngleX);k=0;}
while(Serial.available()){
count=Serial.read();
if (count=='A') {digitalWrite(DIR,HIGH);pulse();k=2;}
if (count=='C'){digitalWrite(DIR,LOW);pulse();delay(1);k=2;}
if (count=='S'){Serial.println(kalAngleX);}
if (count=='Z'){k=1;}
//Serial.println('D');
}
if (k==10){
if (kalAngleX<0){digitalWrite(DIR,LOW);pulse(500);}
else if (kalAngleX>1.8){digitalWrite(DIR,HIGH);pulse(500);}
else {k=0;Serial.println('D');}
}
rep=(rep+1)%500;
if (k==22 && rep==0){Serial.println(kalAngleX);k=0;}
if ((m==0 &&(millis()-stt)>=5000)){m=1;stt=millis();cnt=1;stt1=stt;}
if (m==1){
if (cnt<26 && ((millis()-stt)>=100)){digitalWrite(DIR,LOW);pulse();cnt++;stt=millis();}
if (cnt>=26){m=2;}
}
if (m==2 &&((millis()-stt)>=5000)){m=3;stt=millis();cnt=1;}
if (m==3){
if (cnt<26 && ((millis()-stt)>=100)){digitalWrite(DIR,HIGH);pulse();cnt++;stt=millis();}
if (cnt>=26)m=0;
}
Serial.println(kalAngleX);
//Serial.println(millis()-sr);Serial.println(stt1);
}
int pulse(int interval=1){
digitalWrite(STEP,LOW);
delay(interval);
digitalWrite(STEP,HIGH);
delay(interval);
}
double IMU(){
sr=millis();
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;}
This is the code. I am using 3 MPU6050 units through Mega2560. Each AD0 pin is connected to arduino I/O as output.
Well ok. That's what it does alright. Perhaps it's an error, or maybe they are running a single iteration to set up all the variables correctly.
Maruthi_Garnepudi:
This is the code. I am using 3 MPU6050 units through Mega2560. Each AD0 pin is connected to arduino I/O as output.
I remain of the view I expressed in Reply #5
...R
Robin2:
I remain of the view I expressed in Reply #5...R
I agree that it's silly. That is not how I would set up the variables.
But I can't think of another reason to call loop() from setup().
In the abstract, suppose that in setup() there was quite a bit of some reason to do (once or tentatively or whatever) a bunch of code that will also be used in loop.
In this present case, if it isn't an error in the original, the programmer notices that loop() is exactly what and all he wants to do that one time for whatever reason.
Any time you see such modularity it seems like it would be clearer to make a function() of that common code which could be called both from setup() and loop().
Then me might wonder less and not need to speculate or dig in deeper. Just sayin'.
TBH the code gave me a near-instant headache. I hope caffeine will help. The headache, not the code.
a7
alto777:
In the abstract, suppose that in setup() there was quite a bit of some reason to do (once or tentatively or whatever) a bunch of code that will also be used in loop.In this present case, if it isn't an error in the original, the programmer notices that loop() is exactly what and all he wants to do that one time for whatever reason.
Any time you see such modularity it seems like it would be clearer to make a function() of that common code which could be called both from setup() and loop().
Then me might wonder less and not need to speculate or dig in deeper. Just sayin'.
TBH the code gave me a near-instant headache. I hope caffeine will help. The headache, not the code.
a7
Right. That's what I'm thinkng - the rationale/logic part, not the caffeine.
Naw, caff too. :;
Even if it worked perfectly like that, this is terrible code to release publicly. Such a happenstance would almost never be arrived upon accidentally. It must have been intentionally constructed, clever code.Simply showing off. With the long lists of initial variable contents this program has, surely they could have put them in right the first time (even if that entailed running some code to read a pin for example.)
Instead the effect is to obfuscate the logic, rather than to elucidate. Completely defeating the purpose of a high level language. Perhaps this should have been presented as some sort of ASCII encoded binary file.
(And use an obscure file format too. Like .hqx)
Maruthi_Garnepudi:
The problem here is I want to call loop() from setup() each time a sensor is being read. But once the loop() initiates it doesn't returning to for loop in setup().
Your for loop in setup only does a digitalWrite. Use the tools -> auto format option in the IDE to properly indent your code.
void setup() {
...
...
for (i = 1; i <= n; i++) {
digitalWrite(pin[i], HIGH);
}
i = 1;
while (i <= n) {
...
...
And do yourself a favour and do not give global variables a single letter name; trying to find where you possibly modify the variable i is a mission; the letter i only occurred 376 times in your code so I did not even try.
I further suggest that you put debug print statements in crucial places.
suppose that in setup() there was quite a bit of some reason to do (once or tentatively or whatever) a bunch of code that will also be used in loop.
Then put the code in a function and call it from wherever it is required. No need for all the nonsense of calling loop() repeatedly from setup()
alto777:
Any time you see such modularity it seems like it would be clearer to make a function() of that common code which could be called both from setup() and loop().
+1
...R
for(i=1;i<=n;i++){digitalWrite(pin[i]Oops