基于树莓派的四轴[仅基本功能]
来源：互联网 发布：淘宝代理怎么做步骤 编辑：程序博客网 时间：2024/05/16 06:22
github：https://github.com/Nonikka/Quadcopter
博客的文章地址：http://futuregazer.me/artical/5
上位机是Python 下位机是C，CPP
#include <wiringPi.h>#include <pthread.h>#include <sys/time.h>#include <stdio.h>#include <stdlib.h>#include <unistd.h>#include <signal.h>#include <string.h>#include <errno.h>#include <wiringSerial.h>#include <math.h>#include <wiringPiI2C.h>#include <stdint.h>#include <sys/types.h>#include <sys/socket.h>#include <netinet/in.h>#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20.h"#include "pca9685.h"#define DEFAULT_PORT 8099 #define MAXLINE 4096 #define PIN_BASE 300#define HERTZ 500#define PinNumber1 0 #define PinNumber2 1 #define PinNumber3 8 //放里面省的被浆卷到#define PinNumber4 3 #define IMU_UPDATE_DT 0.01#define MAX_ACC 0.59#define OUTPUT_READABLE_YAWPITCHROLL#define OUTPUT_READABLE_WORLDACCEL// MPU control/status varsbool dmpReady = false; // set true if DMP init was successfuluint8_t mpuIntStatus; // holds actual interrupt status byte from MPUuint8_t devStatus; // return status after each device operation (0 = success, !0 = error)uint16_t packetSize; // expected DMP packet size (default is 42 bytes)uint16_t fifoCount; // count of all bytes currently in FIFOuint8_t fifoBuffer[64]; // FIFO storage bufferint count_time = 0;// orientation/motion varsQuaternion q; // [w, x, y, z] quaternion containerVectorInt16 aa; // [x, y, z] accel sensor measurementsVectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurementsVectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurementsVectorFloat gravity; // [x, y, z] gravity vectorfloat euler[3]; // [psi, theta, phi] Euler angle containerfloat ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector// packet structure for InvenSense teapot demouint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };float Default_Acc = 0.03,Pid_Pitch=0,Pid_Roll=0,Pid_Yaw=0,Accelerator,Roll,Pitch,Yaw,pid_in,pid_error,Roll_PError,Pitch_PError,Yaw_PError,pregyro,Acceleration[3],AngleSpeed[3],Angle[3],DutyCycle[4],Inital_Yaw[7],Inital_Roll[7],Inital_Pitch[7],Filter_Roll[10],Filter_Pitch[10];int All_Count=0,START_FLAG=0,Inital=0,PID_ENABLE=0,_axis[6],filter_count;//遥控器传来的轴unsigned int TimeNow,TimeStart,TimeLastGet;void PWMOut(int pin, float pwm);MPU6050 mpu;struct PID{ float kp; //< proportional gain调整比例参数 float ki; //< integral gain调整积分参数 float kd; //< derivative gain调整微分参数 float pregyro; //float desired; //< set point 期望值 float integ; //< integral积分参数 float iLimit; //< integral limit积分限制范围 float deriv; //< derivative微分参数 float preerror; //< previous error 上一次误差 float output; float error; //< error 误差 float lastoutput;} ;PID Roll_Suit;PID Pitch_Suit;PID Yaw_Suit;void Pid_Inital(){ Roll_Suit.kp = 0.0056;//0.0068有点大 0.064有点大 Roll_Suit.ki = 0.000; Roll_Suit.kd = 0.00175;//跟着0.0018改 Roll_Suit.pregyro =0; //Roll_Suit.desired = 1; Roll_Suit.integ=0; Roll_Suit.iLimit =8; Roll_Suit.deriv=0; Roll_Suit.output = 0.00; Roll_Suit.lastoutput=0; Pitch_Suit.kp = 0.0056; Pitch_Suit.ki = 0.000; Pitch_Suit.kd = 0.00175; Pitch_Suit.pregyro =0; //Pitch_Suit.desired = -0.7; Pitch_Suit.integ=0; Pitch_Suit.iLimit =8; Pitch_Suit.deriv=0; Pitch_Suit.lastoutput=0; Pitch_Suit.output = 0.00; Yaw_Suit.kp = 0.003; Yaw_Suit.kd = 0.002;}float Pid_Calc(PID &pidsuite,float measured,float desired,float Inital_Error){ //pidsuite.preerror = pidsuite.error;//更新前一次偏差 pidsuite.error = desired - measured + Inital_Error ;//偏差：期望-测量值 pidsuite.error = pidsuite.error * 0.88 + pidsuite.preerror * 0.12; //pid_error = pidsuite.error;//debug用 pidsuite.integ += pidsuite.error * IMU_UPDATE_DT;//偏差积分，IMU_UPDATE_DT也就是每调整漏斗大小的步辐 if (pidsuite.integ >= pidsuite.iLimit)//作积分限制 { pidsuite.integ = pidsuite.iLimit; } else if (pidsuite.integ < -pidsuite.iLimit) { pidsuite.integ = -pidsuite.iLimit; } pidsuite.deriv = (pidsuite.error - pidsuite.preerror) / 0.01;//微分 应该可用陀螺仪角速度代替 pidsuite.preerror = pidsuite.error;//debug用 更新前一次偏差 AngleSpeed[0] = pidsuite.deriv; if (fabs(pidsuite.deriv) < 20 ) { if (fabs(pidsuite.deriv) < 10 ) { pidsuite.deriv = pidsuite.deriv * 0.8; } else { pidsuite.deriv = pidsuite.deriv * 0.9; } } pidsuite.output = (pidsuite.kp * pidsuite.error) + (pidsuite.ki * pidsuite.integ) + (pidsuite.kd * pidsuite.deriv); pid_in = pidsuite.output; pregyro = pidsuite.pregyro; if (pidsuite.output > 0.12) { pidsuite.output = 0.12; } if (pidsuite.output < -0.12) { pidsuite.output = -0.12; } //output = output * 0.9 + lastoutput * 0.1; if (fabs(pidsuite.error) < 0.3 ) { pidsuite.output = pidsuite.lastoutput * 0.5; } pidsuite.lastoutput = pidsuite.output; if(PID_ENABLE = 0) { return 0; } return pidsuite.output;}float average_filter(float filter_input ,float (&Filter)[10]){ float filter_output; Filter[9] = filter_input; filter_output = 0.65 * Filter[8] + 0.14 * Filter[6] + 0.11 * Filter[4] + 0.06 * Filter[2] + 0.04 * Filter[0]; for(filter_count=9;filter_count>0;filter_count--) { Filter[filter_count-1]=Filter[filter_count]; } return filter_output;}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] = average_filter(ypr[1] * 180/M_PI,Filter_Pitch);//此为Pitch Angle[0] = average_filter(ypr[2] * 180/M_PI,Filter_Roll);//此为Roll // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion Pid_Roll = Pid_Calc(Roll_Suit,Angle[0],0.68 - 11 * _axis[1] * 0.01,0.38); Pid_Pitch = Pid_Calc(Pitch_Suit,Angle[1],-0.55 - 11 * _axis[2] * 0.01,-0.13); Pid_Yaw = Pid_Calc(Yaw_Suit,Angle[2],0,Inital_Yaw[1]); All_Count = All_Count + 1; Default_Acc = Default_Acc + _axis[0] * 0.0001 * 0.052;//0.04太小 TimeNow = millis(); if (abs(TimeNow - TimeLastGet) > 800) { if(Default_Acc > 0.4) { Default_Acc = 0.46; _axis[1] = 0; _axis[2] = 0;//前后左右置零 } else { Default_Acc = 0.03; } } if (Default_Acc >= MAX_ACC) { Default_Acc = MAX_ACC; } if (Default_Acc <= 0.03) { Default_Acc = 0.03; } DutyCycle[0] = Default_Acc - Pid_Roll - Pid_Pitch - Pid_Yaw; DutyCycle[1] = Default_Acc - Pid_Roll + Pid_Pitch + Pid_Yaw; DutyCycle[2] = Default_Acc + Pid_Roll - Pid_Pitch + Pid_Yaw; DutyCycle[3] = Default_Acc + Pid_Roll + Pid_Pitch - Pid_Yaw; PWMOut(PinNumber1,DutyCycle[0]); PWMOut(PinNumber2,DutyCycle[1]); PWMOut(PinNumber3,DutyCycle[2]); PWMOut(PinNumber4,DutyCycle[3]); } }}//1油门 2前后 3左右 4旋转 5预留 6预留 每个三位void* serial_DL22(void*){ int fd,counter=0; //int Num_Avail; unsigned char Re_buf[19]; unsigned char ucStr[18]; char axis1[5],axis2[5],axis3[5],axis4[5]; if ((fd = serialOpen ("/dev/ttyAMA0", 115200)) < 0) { fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ; return 0 ; } for (;;) { Re_buf[counter]=serialGetchar(fd); if(Re_buf[0]!=0x55) // 0x55 = U { memset(Re_buf, 0, 18*sizeof(char)); counter = 0; } else { counter++; if(counter==18) //接收到11个数据 { counter=0; //重新赋值，准备下一帧数据的接收 TimeLastGet = millis(); ucStr[0]=Re_buf[1]; ucStr[1]=Re_buf[2]; ucStr[2]=Re_buf[3]; ucStr[3]=Re_buf[4]; ucStr[4]=Re_buf[5]; ucStr[5]=Re_buf[6]; ucStr[6]=Re_buf[7]; ucStr[7]=Re_buf[8]; ucStr[8]=Re_buf[9]; ucStr[9]=Re_buf[10]; ucStr[10]=Re_buf[11]; ucStr[11]=Re_buf[12]; ucStr[12]=Re_buf[13]; ucStr[13]=Re_buf[14]; ucStr[14]=Re_buf[15]; ucStr[15]=Re_buf[16]; ucStr[16]=Re_buf[17]; ucStr[17]=Re_buf[18]; axis1[0] = Re_buf[1]; axis1[1] = Re_buf[2]; axis1[2] = Re_buf[3]; axis1[3] = Re_buf[4]; axis1[4] = '\0'; _axis[0] = atoi(axis1);//油门控制 axis2[0] = Re_buf[5]; axis2[1] = Re_buf[6]; axis2[2] = Re_buf[7]; axis2[3] = Re_buf[8]; axis2[4] = '\0'; _axis[1] = atoi(axis2);//前后控制 roll axis3[0] = Re_buf[9]; axis3[1] = Re_buf[10]; axis3[2] = Re_buf[11]; axis3[3] = Re_buf[12]; axis3[4] = '\0';//不加会导致转换错误 _axis[2] = atoi(axis3);//左右控制 pitch printf("recv from client: %s %d %d %d\n\n",ucStr,_axis[0],_axis[1],_axis[2]); memset(Re_buf, 0, 18*sizeof(char)); } } }}void PWMOut(int pin, float pwm)//pwm valaue:0~1{ int outpwm = pwm * 2048 + 2048; //softPwmWrite(pin, (int)outpwm); pwmWrite(PIN_BASE + pin,outpwm);}int main(){ pthread_t mpu6050,joystick;//transport int ret; Pid_Inital(); PID_ENABLE = 1; 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); } 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); delay(1000); //getchar(); START_FLAG = 1; PWMOut(PinNumber1,0.06); PWMOut(PinNumber2,0.06); PWMOut(PinNumber3,0.06); PWMOut(PinNumber4,0.06); delay(500); TimeStart = millis(); /*********************/ /* ret = pthread_create(&transport,NULL,KeyBoard,NULL); if(ret!=0) { printf ("Create KeyBoard thread error!\n"); exit (1); }*/ ret = pthread_create(&joystick,NULL,serial_DL22,NULL);//启动serial手柄线程 if(ret!=0) { printf ("Create joystick thread error!\n"); exit (1); } delay(4200); Inital_Yaw[1] = Angle[2]; while(1) { system("clear"); printf("Pid_Roll:%.4f Pid_Yaw:%.4f pid_error:%.3f pregyro %.3f All_Count: %d",Pid_Roll,Pid_Yaw,pid_error,pregyro,All_Count); printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]); printf("Default_Acc:%.3f gyro： roll :%.2f\n",Default_Acc,AngleSpeed[0]); fflush(stdout); }}
0 0
- 基于树莓派的四轴[仅基本功能]
- iphone（基本功能的实现（四…
- 基于SVG技术实现WebGIS的基本功能
- BaiduMap初体验(四) 使用百度地图的基本功能
- 基于SVG技术实现WebGIS的基本功能（1）
- 基于SVG技术实现WebGIS的基本功能（2）
- 基于SSM框架实现数据库的基本功能（一）
- mineportal2：基于mailinbox，一个基本功能完备的整合个人件
- 基于Linux的TCP网络编程--仅函数使用。基础
- [android开发之内容更新类APP]四、项目的基本功能之布局(续)
- dotnetnuke的基本功能
- 测试vpn的基本功能
- 完成MyJFrame的基本功能
- ADO.NET的基本功能
- 路由器的基本功能
- CRM软件的基本功能
- ESB的基本功能
- WINCE实现的基本功能
- Linux下system()函数返错，errno是ECHILD http://blog.csdn.net/taolinke/article/details/8057335
- 数据库之索引
- java 声明和动态创建数组
- Sping管理事务实现方式
- tomcat重启脚本
- 基于树莓派的四轴[仅基本功能]
- PAT(甲级)1062
- Settings
- java 代码中实现 TextView的 DrawableTop属性
- HD1003（Max Sum）
- 亿级Web系统搭建：单机到分布式集群
- 数据库管理技术及理论--0.导论
- 文章标题
- 三色旗
