void initialize_imu() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
// **************************************************************
// It is best to configure I2C to 400 kHz.
// If you are using an Arduino DUE, modify the variable TWI_CLOCK to 400000, defined in the file:
// c:/Program Files/Arduino/hardware/arduino/sam/libraries/Wire/Wire.h
// If you are using any other Arduino instead of the DUE, uncomment the following line:
//TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) //This line should be commented if you are using Arduino DUE
// **************************************************************
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(250000);
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// TODO: Compute these parameters
// mpu.setXAccelOffset(-1600);
// mpu.setYAccelOffset(-180);
// mpu.setZAccelOffset(650);
// mpu.setXGyroOffset(0);
// mpu.setYGyroOffset(0);
// mpu.setZGyroOffset(0);
mpu.setFullScaleGyroRange(0);
calibrate_imu();
// Magnetometer configuration
mpu.setI2CMasterModeEnabled(0);
mpu.setI2CBypassEnabled(1);
Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
Wire.write(0x02);
Wire.write(0x00); // Set continuous mode
Wire.endTransmission();
delay(5);
Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
Wire.write(0x00);
Wire.write(B00011000); // 75Hz
Wire.endTransmission();
delay(5);
mpu.setI2CBypassEnabled(0);
// X axis word
mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
mpu.setSlaveEnabled(0, true);
mpu.setSlaveWordByteSwap(0, false);
mpu.setSlaveWriteMode(0, false);
mpu.setSlaveWordGroupOffset(0, false);
mpu.setSlaveDataLength(0, 2);
// Y axis word
mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
mpu.setSlaveEnabled(1, true);
mpu.setSlaveWordByteSwap(1, false);
mpu.setSlaveWriteMode(1, false);
mpu.setSlaveWordGroupOffset(1, false);
mpu.setSlaveDataLength(1, 2);
// Z axis word
mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
mpu.setSlaveEnabled(2, true);
mpu.setSlaveWordByteSwap(2, false);
mpu.setSlaveWriteMode(2, false);
mpu.setSlaveWordGroupOffset(2, false);
mpu.setSlaveDataLength(2, 2);
mpu.setI2CMasterModeEnabled(1);
mpu.setDLPFMode(6);
}
void* gyro_acc(void*)
{
//float kp = 0.00375,ki = 0.0000,kd = 0.00076;
float kp = 0.0068,ki = 0.000,kd = 0.0018;
//0030 0088 0014 有偏角 p0.0031偏角更大 0.0029也是 i=0 小偏角 p0.00305 d0.00143 不错 i0.0005 偏角变大
//0032 0017
float pregyro =0;
float desired = 0;
//double error;
float integ=0;//integral积分参数
float iLimit =8 ;
float deriv=0;//derivative微分参数
float prevError=0;
float lastoutput=0;
//float Piddeadband=0.3;
// initialize device
printf("Initializing I2C devices...\n");
mpu.initialize();
// verify connection
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true);
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
}
/*****************************************************/
while(1)
{
if (START_FLAG == 0)
{
delay(200);
}
if (START_FLAG == 1)
{
break;
}
}
delay(50);
for(;;)
{
if (!dmpReady) return 0;
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (fifoCount >= 42)
{
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//printf("ypr %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
Angle[2] = ypr[0] * 180/M_PI;
Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
Angle[0] = ypr[2] * 180/M_PI;//此为Roll
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
//printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z);
AngleSpeed[0] = aaWorld.x;
AngleSpeed[1] = aaWorld.y;
AngleSpeed[2] = aaWorld.z;
/****************************读取完毕*********************************/
error = desired - Angle[0];//偏差：期望-测量值
All_Count = All_Count + 1;
error = error * 0.88 + prevError * 0.12;
/*
if (fabs(prevError - error ) > 12)
{
error = prevError;
}*/
integ += error * IMU_UPDATE_DT;//偏差积分，IMU_UPDATE_DT也就是每调整漏斗大小的步辐
if (integ >= iLimit)//作积分限制
{
integ = iLimit;
}
else if (integ < -iLimit)
{
integ = -iLimit;
}
deriv = (error - prevError) / IMU_UPDATE_DT;//微分 应该可用陀螺仪角速度代替
AngleSpeed[0] = deriv;
if (fabs(deriv) < 20 )
{
if (fabs(deriv) < 10 )
{
deriv = deriv * 0.8;
}
else
{
deriv = deriv * 0.9;
}
}
//if(deriv
//roll.deriv = -gyro;//注意是否跟自己的参数方向相反，不然会加剧振荡
//deriv = -AngleSpeed[0];
/*
if (fabs(pregyro - deriv) > 20)
{
deriv = deriv * 0.5 + pregyro * 0.5;
}
*/
output = (kp * error) + (ki * integ) + (kd * deriv);
prevError = error;//更新前一次偏差
pregyro = deriv;
if (output > 0.16)
{
output = 0.16;
}
if (output < -0.16)
{
output = -0.16;
}
Pid_Roll = output;
//output = output * 0.9 + lastoutput * 0.1;
if (fabs(error) < 0.3 )
{
output = lastoutput * 0.5;
}
lastoutput = output;
DutyCycle[0] = Default_Acc - output;
DutyCycle[1] = Default_Acc - output;
//DutyCycle[0] = Default_Acc;
//DutyCycle[1] = Default_Acc;
DutyCycle[2] = Default_Acc + output;
DutyCycle[3] = Default_Acc + output;
//DutyCycle[2] = Default_Acc;
//DutyCycle[3] = Default_Acc;
PWMOut(PinNumber1,DutyCycle[0]);
PWMOut(PinNumber2,DutyCycle[1]);
PWMOut(PinNumber3,DutyCycle[2]);
PWMOut(PinNumber4,DutyCycle[3]);
}
}
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
Pid_Inital();
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
TimeNow = millis();
system("clear");
printf("Pid_Roll:%.4f pid_error:%.3f pid_pError:%.3f pregyro %.3f All_Count: %d",Pid_Roll,pid_error,Roll_PError,pregyro,All_Count);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro： roll :%.2f\n",Default_Acc,AngleSpeed[0]);
fflush(stdout);
}
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
getchar();
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
//Pid_Pitch = PidUpdate(Angle[1],-1.6,AngleSpeed[1]);
//Pid_Roll = PidUpdate_roll(Angle[0],0,AngleSpeed[0]);
TimeNow = millis();
system("clear");
//delay(100);
printf("Pid_Roll:%.4f error %.3f All_Count: %d time:%d\n",Pid_Roll,error,All_Count,TimeNow - TimeStart);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro：Pitch：%.2f roll :%.2f\n",Default_Acc,AngleSpeed[1],AngleSpeed[0]);
fflush(stdout);
//DutyCycle[3] = Default_Acc + Pid_Pitch - Pid_Roll;//+yaw
//DutyCycle[2] = Default_Acc - Pid_Pitch + Pid_Roll;//+yaw
//DutyCycle[1] = Default_Acc + Pid_Pitch + Pid_Roll;//-yaw
//DutyCycle[0] = Default_Acc - Pid_Pitch - Pid_Roll;//-yaw
}
}
void* gyro_acc(void*)
{
int i = 0;
// initialize device
printf("Initializing I2C devices...\n");
mpu.initialize();
// verify connection
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
mpu.setI2CMasterModeEnabled(false);
mpu.setI2CBypassEnabled(true);
// load and configure the DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
//attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
printf("DMP ready!\n");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
printf("DMP Initialization failed (code %d)\n", devStatus);
return 0;
}
/*****************************************************/
while(1)
{
if (START_FLAG == 0)
{
delay(200);
}
if (START_FLAG == 1)
{
break;
}
}
delay(50);
for(;;)
{
if (!dmpReady) return 0;
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (fifoCount >= 42)
{
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//printf("ypr %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
Angle[2] = ypr[0] * 180/M_PI;
Angle[1] = ypr[1] * 180/M_PI;//此为Pitch
Angle[0] = ypr[2] * 180/M_PI;//此为Roll
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
/*
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
//printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z);
//AngleSpeed[0] = aaWorld.x;
//AngleSpeed[1] = aaWorld.y;
//AngleSpeed[2] = aaWorld.z;
*/
/****************************读取完毕*********************************/
if (Inital <= 300)
{
Inital ++;
if (Inital % 98 == 1)
{
Inital_Roll[i] = Angle[0];
Inital_Pitch[i] = Angle[1];
Inital_Yaw[i] = Angle[2];
printf("Roll:%.2f Pitch:%.2f Yaw:%.2f",Inital_Roll[i],Inital_Pitch[i],Inital_Yaw[i]);
i++;
printf("%d\n",Inital);
fflush(stdout);
if (i == 3)
{
Inital_Yaw[3] = (Inital_Yaw[0] + Inital_Yaw[1] + Inital_Yaw[2]) / 3;
Inital_Roll[3] =(Inital_Roll[0] + Inital_Roll[1] + Inital_Roll[2]) / 3;
Inital_Pitch[3] = (Inital_Pitch[0] + Inital_Pitch[1] + Inital_Pitch[2]) / 3;
}
}
}
else
{
Pid_Roll = Pid_Calc_R(Roll_Suit,Angle[0]);
Pid_Pitch = Pid_Calc_P(Pitch_Suit,Angle[1]);
Pid_Yaw = Pid_Calc_Y(Yaw_Suit,Angle[2],Inital_Yaw[3]);
All_Count = All_Count + 1;
DutyCycle[0] = Default_Acc - Pid_Roll - Pid_Pitch; //- Pid_Yaw;
DutyCycle[1] = Default_Acc - Pid_Roll + Pid_Pitch; //+ Pid_Yaw;
//DutyCycle[0] = Default_Acc;
//DutyCycle[1] = Default_Acc;
DutyCycle[2] = Default_Acc + Pid_Roll - Pid_Pitch; //+ Pid_Yaw;
DutyCycle[3] = Default_Acc + Pid_Roll + Pid_Pitch; //- Pid_Yaw;
//DutyCycle[2] = Default_Acc;
//DutyCycle[3] = Default_Acc;
PWMOut(PinNumber1,DutyCycle[0]);
PWMOut(PinNumber2,DutyCycle[1]);
PWMOut(PinNumber3,DutyCycle[2]);
PWMOut(PinNumber4,DutyCycle[3]);
}
}
}
}
