Values of c++ array keep getting switched - c++

Question

I have an arduino program that runs a servo when it's at an odd angle, but after five seconds if still at that odd angle set it as the new center. the problem is that for some reason when I'm setting the angles of the current rotation to be the new center, the values get mixed up. Furthermore, the values of the center seem to be updating before the five seconds has even passed. I think this is something to do with the compiler. My code is here:
/*
Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
by Dejan, https://howtomechatronics.com
*/
#include <Wire.h>
#include <Servo.h>
#define SERVO_PIN 9 //PWM pin that is connected to the servo
Servo demoServo; //create a servo object
int servoAngle = 0; //servo angle which can vary from 0 - 180
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float rot[1];
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
bool shake;
int shakeTime = 0;
float stablePos[1];
void setup() {
Serial.begin(19200);
Wire.begin(); // Initialize comunication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
demoServo.attach(SERVO_PIN);
delay(20);
stablePos[0] = 0;
stablePos[1] = 0;
rot[0] = 0;
rot[1] = 1;
}
void setIMU(float rot[])
{
// === Read acceleromter data === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.93; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - 4.23; // AccErrorY ~(-1.58)
// === Read gyroscope data === //
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
Wire.beginTransmission(MPU);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// Correct the outputs with the calculated error values
GyroX = GyroX + 0.43; // GyroErrorX ~(-0.56)
GyroY = GyroY + 0.63; // GyroErrorY ~(2)
GyroZ = GyroZ + 1.67; // GyroErrorZ ~ (-0.8)
// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Complementary filter - combine acceleromter and gyro angle values
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
rot[0] = pitch-10 > rot[0] || pitch+10 < rot[0] ? pitch : rot[0];
rot[1] = roll-10 > rot[1] || roll+10 < rot[1] ? roll : rot[1];
}
void loop() {
setIMU(rot);
Serial.print("STABLE AT:");
Serial.print(stablePos[0]);
Serial.print("/");
Serial.print(stablePos[1]);
Serial.print("\t CURRENTLY AT:");
Serial.print(rot[0]);
Serial.print("/");
Serial.println(rot[1]);
shake = (rot[0]>stablePos[0]+20 || rot[0]<stablePos[0]-20) ? true : (rot[1]>stablePos[1]+20 || rot[1]<stablePos[1]-20);
shakeTime = shake && shakeTime == 0 ? millis() + 5000 : shakeTime;
if (shakeTime < millis() && shakeTime != 0){
shakeTime = 0;
shake = false;
stablePos[0] = rot[0];
stablePos[1] = rot[1];
}
Serial.println(shake);
//Serial.println(shakeTime/1000);
demoServo.write(shake ? 180 : 0);
}
and the output of a test can be seen here:
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/0.00
0
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/0.00
0
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/-0.00
0
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/-0.14
0
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/-0.26
0
STABLE AT:0.00/0.00 CURRENTLY AT:0.00/0.05
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/0.41
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/-0.00
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/0.16
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/0.00
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/1.49
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/1.86
0
STABLE AT:0.00/10.98 CURRENTLY AT:10.98/0.00
0
STABLE AT:0.00/21.30 CURRENTLY AT:21.30/0.63
1
STABLE AT:0.00/21.30 CURRENTLY AT:21.30/2.57
1
STABLE AT:0.00/21.30 CURRENTLY AT:21.30/2.70
1
STABLE AT:0.00/21.30 CURRENTLY AT:21.30/2.69
1
STABLE AT:0.00/31.95 CURRENTLY AT:31.95/2.56
1
STABLE AT:0.00/31.95 CURRENTLY AT:31.95/-0.00
1
STABLE AT:0.00/31.95 CURRENTLY AT:31.95/-0.00
1
as you can see, not only does the stable position update to 10.98 before the shake boolean gets set to two, but the placement is reversed as well.

Answer 1

float stablePos[1]; is an array of one element; the only valid index is stablePos[0]. By accessing stablePos[1], your program exhibits undefined behavior. Same with rot.