C ++数组的值不断变化

编程问答 2022-08-16

问题描述

我有一个arduino程序，当它处于奇数角时运行伺服，但是五秒钟后（如果仍以该奇数角）将其设置为新中心。问题是由于某种原因，当我将当前旋转角度设置为新的中心时，这些值会混合在一起。此外，中心值似乎在五秒钟之前就已更新。我认为这与编译器有关。我的代码在这里：

copy

和测试的输出可以在这里看到：

/*
   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))) * 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,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); 
  
}

如您所见，不仅在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 布尔值设置为2之前，稳定位置会更新为10.98，而且位置也会相反。

解决方法

float stablePos[1];是一个元素的数组；唯一有效的索引是stablePos[0]。通过访问stablePos[1]，您的程序表现出未定义的行为。与rot相同。

arraysarraysc++compilation