コードに問題があり、数時間コードを使用して問題を特定しようとしていますが、原因を特定できません。画面に出力すると、すべての値がゼロに等しくなるはずですが、代わりに、変数はゼロになるはずのときにangleY
出力を続けます。34244
この価値がどこから来ているのか、そしてその理由を誰かに教えてもらえないかと思います。すべての変数の値を出力する行printf("current rate: %d angle rate: %d angle rate: %d previous rate: %d \n",currentRateY, angleRateY, angleY, previousRateY);
があるので、問題を特定でき、すべて0
の変数は変数まで残りangleY
ます。私のコードは以下の通りです:
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <time.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
int fd;
short x = 0;
short y = 0;
short z = 0;
int main (){
fd = wiringPiI2CSetup(0x69); // I2C address of gyro
wiringPiI2CWriteReg8(fd, CTRL_REG1, 0x1F); //Turn on all axes, disable power down
wiringPiI2CWriteReg8(fd, CTRL_REG3, 0x08); //Enable control ready signal
wiringPiI2CWriteReg8(fd, CTRL_REG4, 0x80); // Set scale (500 deg/sec)
delay(200); // Wait to synchronize
void getGyroValues (){
int MSB, LSB;
LSB = wiringPiI2CReadReg8(fd, 0x28);
MSB = wiringPiI2CReadReg8(fd, 0x29);
x = ((MSB << 8) | LSB);
MSB = wiringPiI2CReadReg8(fd, 0x2B);
LSB = wiringPiI2CReadReg8(fd, 0x2A);
y = ((MSB << 8) | LSB);
MSB = wiringPiI2CReadReg8(fd, 0x2D);
LSB = wiringPiI2CReadReg8(fd, 0x2C);
z = ((MSB << 8) | LSB);
}
for (int i=0;i<1000;i++){
getGyroValues();
int previousRateZ = z /114;
int previousRateY = y /114;
int previousRateX = x /114;
delay(100);
getGyroValues();
int currentRateZ = z /114;
int currentRateY = y /114;
int currentRateX = x /114;
int angleRateZ = ((long)(previousRateZ + currentRateZ) * 105)/1000;
int angleRateY = ((long)(previousRateY + currentRateY) * 105)/1000;
int angleRateX = ((long)(previousRateX + currentRateX) * 105)/1000;
int angleZ = angleZ + angleRateZ;
int angleY = angleY + angleRateY;
int angleX = angleX + angleRateX;
printf("current rate: %d angle rate: %d angle rate: %d previous rate: %d \n",currentRateY, angleRateY, angleY, previousRateY);
delay(100);
if(i == 1){
printf("Z equals: %d\n", angleZ);
printf("Y equals: %d\n", angleY);
printf("X equals: %d\n", angleX);
i=0;
}
}
};