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]); } } } }