设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › 51单片机 › 基于STC15W的风力摆控制系统设计 带视频与程序（电赛测 ...
返回列表 发新帖
查看: 2833|回复: 1
打印 上一主题 下一主题
收起左侧

基于STC15W的风力摆控制系统设计 带视频与程序（电赛测控题）

[复制链接]
跳转到指定楼层
楼主
ID:192220 发表于 2017-9-6 16:26 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
风力摆视频演示：


元件清单：



单片机源程序如下:
  1. //========================================================================
  2. //        城建学院电子实验室
  3. //        修改日期2017.07.30
  4. //        版本V1.0
  5. //========================================================================
  6. //特别声明：此程序
  7. //         
  8. //
  9. //
  10. //                                 MCU工作频率:24MHz
  11. //=========================================================================

  13. #include <STC15F2K60S2.h>        //STC15系列通用头文件
  14. #include <intrins.h>                //STC特殊命令符号声明
  15. #include <MPU6050.H>                //MPU6050数字陀螺仪
  16. #include <STC15W4KPWM.H>        //单片机所有IO口初始化-PWM口初始化
  17. #include <IMU.H>                        
  18. #include <math.H>

  20. bit busy,flag=0;
  21. int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0;                //匿名科技上位机
  22. //==================================================//
  23. //  PID算法变量
  24. //==================================================//
  25. int speed1=0,speed2=0,speed3=0,speed4=0;//电机速度参数
  26. int PWM1=0,PWM2=0,PWM3=0,PWM4=0;                //加载至PWM模块的参数
  27. float Angle=0;
  28. char mode=1;
  29. //*****************角度参数*************************************************
  30. float Last_Angle_gx=0;                                          //外上一次陀螺仪数据
  31. float Last_Angle_gy=0;                                                  //上一次陀螺仪数据
  32. double Gyro_y=0,Gyro_x=0,Gyro_z=0;        //Y轴陀螺仪数据暂存
  33. double Accel_x=0,Accel_y=0,Accel_z=0;          //X轴加速度值暂存
  34. double Angle_ax=0,Angle_ay=0,Angle_az=0;  //由加速度计算的加速度(弧度制)
  35. double Angle_gy=0,Angle_gx=0,Angle_gz=0;  //由角速度计算的角速率(角度制)
  36. float data AngleX=0,AngleY=0;                                  //四元数解算出的欧拉角
  37. double Ax=0,Ay=0;                    //加入遥控器控制量后的角度
  38. double Vx=0,Vy=0;                    //加入遥控器控制量后的角度
  39. float ierrorx=0,ierrory=0; //误差积分 积分根据情况，目前测试情况不需要积分
  40. //****************姿态处理和PID*********************************************
  41. float FR1=0,FR2=0;            //xy轴期望角度
  42. float  data PID_Output;                                //PID最终输出量

  44. float xdata Last_Ax=0;
  45. float xdata Last_Ay=0;
  46. float xdata Last_vx=0;
  47. float xdata Last_vy=0;


  50. float R=0.20f;//摆幅半长 （m）
  51. unsigned int time=0;
  52. float A1=0,A2=0;

  54. sbit key1=P2^0;
  55. sbit key2=P2^1;
  56. sbit key3=P2^2;
  57. sbit key4=P2^3;




  62. //==================================================//
  63. //   PID 手动微调参数值
  64. //==================================================//
  65. // P值适当调，D值可以给很大!!! P值过大系统就会荡

  67. #define Out_XP        4.5f        
  68. #define        Out_XI  0.0f         //I根据情况，目前不需要I
  69. #define        Out_XD  2200.0f        

  71. #define        Out_YP        Out_XP
  72. #define        Out_YI        Out_XI
  73. #define        Out_YD        Out_XD

  75. #define PI 3.15459
  76. #define T 1601//单摆周期(ms) 根据单摆公式用杆长计算

  78. //--------------------------------------------------//
  79. //  PID算法飞控自平衡 函数
  80. //--------------------------------------------------//
  81. void Flight(void)interrupt 1
  82. {
  83.     time+=10;
  84.         Gyro_x = GetData(GYRO_XOUT_H);//读出 X轴陀螺仪数据
  85.         Gyro_y = GetData(GYRO_YOUT_H);//读出 Y轴陀螺仪数据
  86.         Gyro_z = GetData(GYRO_ZOUT_H);//读出 Z轴陀螺仪数据        
  87.         Accel_y= GetData(ACCEL_YOUT_H);//读出 X轴加速度数据
  88.         Accel_x= GetData(ACCEL_XOUT_H);//读出 Y轴陀螺仪数据                  
  89.         Accel_z= GetData(ACCEL_ZOUT_H);//读出 Z轴加速度数据
  90.         //姿态数据算法 （借鉴STC官方算法）
  91.         Last_Angle_gx=Angle_gx;   //储存上一次角速度数据
  92.         Last_Angle_gy=Angle_gy;          //储存上一次角速度数据
  93.         Angle_ax=(Accel_x)/8192;  //加速度处理
  94.         Angle_az=(Accel_z)/8192;  //加速度量程 +-4g/S
  95.         Angle_ay=(Accel_y)/8192;  //转换关系   8192LSB/g
  96.         Angle_gx=(Gyro_x)/65.5+4;   //陀螺仪处理
  97.         Angle_gy=(Gyro_y)/65.5+3;   //陀螺仪量程 +-500度/S
  98.         Angle_gz=(Gyro_z)/65.5;   //转换关系65.5LSB/度

  100. //***********************************四元数解算***********************************
  101.         IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);//0.174533为PI/180 目的是将角度转弧度

  103. //****平衡算法***********************************************************************



  107.     FR1=A1*sin(2*PI*time/T);        //X轴控制
  108.         if(mode==3) FR2=A2*cos(2*PI*time/T); //Y轴控制
  109.         else if(mode==4) FR2=A2*cos(2*PI*time/(T/2)); //Y轴控制
  110.         else   FR2=A2*sin(2*PI*time/T); //Y轴控制
  111.         

  113.         if(time >= 1601)time=0;


  116. //************** MPU6050 X轴指向 ***********************************************************
  117.     Ax=FR1-(AngleX+2);   //X轴偏差

  119.         ich1=Ax;
  120.         ich2=Ay;
  121.         ich3=0;                 
  122.         ich4=0;
  123.         ich5=0;
  124.         ich6=0;

  126.         ierrorx +=Last_Ax-Ax;

  128.     PID_Output = Ax*Out_XP +Out_XI*ierrorx+(Last_Ax-Ax)*Out_XD;        //PID

  130.         if(PID_Output >  1000)        PID_Output =  1000;  //输出量限幅
  131.         if(PID_Output < -1000)        PID_Output = -1000;
  132.         
  133.         speed1 =0+ PID_Output;        speed3 =0- PID_Output;

  135.         Last_Ax=Ax;

  137. //**************MPU6050 Y轴指向**************************************************

  139.           Ay=FR2-(AngleY);  //Y轴偏差

  141.         ierrory +=Last_Ay-Ay;

  143.     PID_Output = Ay*Out_YP +Out_YI*ierrory+(Last_Ay-Ay)*Out_XD;        //PID

  145.         if(PID_Output >  1000)        PID_Output =  1000;  //输出量限幅
  146.         if(PID_Output < -1000)        PID_Output = -1000;
  147.                
  148.         speed2 =0+ PID_Output;        speed4 = 0- PID_Output;
  149.                         
  150.         Last_Ay=Ay;           

  152. //************************PID控制数值***************************************************
  153. //**************将速度参数加载至PWM模块*************************************************        
  154.         //速度参数控制，防止超过PWM参数范围0-1000
  155.         if(speed1>1000)speed1=1000;else if(speed1<0)speed1=0;PWM1=(speed1);                // 5
  156.         if(speed2>1000)speed2=1000;else if(speed2<0)speed2=0;PWM2=(speed2);                // 3
  157.         if(speed3>1000)speed3=1000;else if(speed3<0)speed3=0;PWM3=(speed3);                // 4
  158.         if(speed4>1000)speed4=1000;else if(speed4<0)speed4=0;PWM4=(speed4);                // 2   

  160.         PWM(1000-PWM1,1000-PWM2,1000-PWM3,1000-PWM4);
  161. }
  162.          
  163. //--------------------------------------------------//
  164. //  时间延时 函数
  165. //--------------------------------------------------//
  166. void Delay(unsigned int x)
  167. {
  168.         unsigned int i,j;
  169.     for(i=0;i<x;i++)
  170.     for(j=0;j<250;j++);
  171. }
  172. //--------------------------------------------------//
  173. //  定时器0 初始化函数 V2.0
  174. //--------------------------------------------------//


  177. void Timer0Init(void)        //10毫秒@24.000MHz
  178. {
  179.         TR0 = 0;
  180.         AUXR &= 0x7F;        //定时器时钟12T模式
  181.         TMOD &= 0xF0;        //设置定时器模式
  182.         IE  = 0x82;
  183.         TL0 = 0xE0;                //设置定时初值
  184.         TH0 = 0xB1;                //设置定时初值
  185.         TF0 = 0;                //清除TF0标志
  186.         TR0 = 1;                //定时器0开始计时

  188. }

  190. //--------------------------------------------------//
  191. //  上位机串口 初始化函数 V2.0
  192. //  波特率115200,24MHZ
  193. //--------------------------------------------------//

  195. void Usart_Init()  //波特率115200,24MHZ
  196. {
  197.     SCON = 0x50;        
  198.         AUXR |= 0x40;               
  199.         AUXR &= 0xFE;               
  200.         TMOD &= 0x0F;        
  201.         TL1 = 0xcc;               
  202.         TH1 = 0xFF;               
  203.         ET1 = 0;               
  204.         TR1 = 1;        
  205. }

  207. void SendData(unsigned char dat)
  208. {
  209.     while (busy);            
  210.      busy = 1;
  211.     SBUF = dat;               
  212. }
  213. void Send(int Ax,int Ay,int Az,int Gx,int Gy,int Gz)
  214. {
  215.         unsigned char sum=0;
  216.         ES = 1;  
  217.         SendData(0xAA);   
  218.         SendData(0xAA);   
  219.         SendData(0x02);   
  220.         SendData(12);     
  221.         SendData(Ax>>8);     
  222.         SendData(Ax);  
  223.         SendData(Ay>>8);
  224.         SendData(Ay);
  225.         SendData(Az);
  226.         SendData(Az>>8);
  227.         SendData(Gx);
  228.         SendData(Gx>>8);
  229.         SendData(Gy);
  230.         SendData(Gy>>8);
  231.         SendData(Gz);
  232.         SendData(Gz>>8);
  233.         sum+=0xAA;sum+=0xAA;sum+=0x02;sum+=12;
  234.         sum+=Ax>>8;sum+=Ax;sum+=Ay>>8;sum+=Ay;sum+=Az>>8;sum+=Az;
  235.         sum+=Gx>>8;sum+=Gx;sum+=Gy>>8;sum+=Gy;sum+=Gz>>8;sum+=Gz;
  236.         SendData(sum);  
  237.         ES = 0;   
  238. }


  241. void mode1()
  242. {        
  243.         A1=0;
  244.         A2=0;
  245. }

  247. void mode2()
  248. {        
  249.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A         0.75m摆长
  250.         A2=(atan(R*sin(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A

  252.         /*if(0==key3)
  253.         {
  254.                 Delay(10);
  255.                 if(key3==0) Angle+=15;
  256.                 if(Angle>=360) Angle=0;
  257.                 while(!key1);
  258.         }
  259.         if(key4==0)
  260.         {
  261.                 Delay(10);
  262.                 if(key4==0) Angle-=15;
  263.                 if(Angle<=-360) Angle=0;
  264.                 while(!key4);
  265.         }*/
  266. }

  268. void mode3()
  269. {        
  270.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A         0.75m摆长
  271.         /*if(key3==0)
  272.         {
  273.                 Delay(10);
  274.                 if(key3==0) flag=~flag;
  275.                 while(!key3);
  276.         }*/
  277.         if(!flag)  A2=A1;
  278.         else      A2=0-A1;
  279. }

  281. void mode4()
  282. {        
  283.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A         0.75m摆长
  284.         A2=A1/2;
  285. }

  287. //--------------------------------------------------//
  288. //  程序 主函数
  289. //--------------------------------------------------//
  290. void main(void)
  291. {
  292.         PWMGO();                //初始化PWM
  293.         Usart_Init();//串口初始化
  294.         Delay(10);    // 延时 100
  295.         InitMPU6050();        //初始化MPU-6050
  296.         Delay(100);    // 延时 100
  297.         Timer0Init();        //初始化定时器        
  298.         EA = 1;
  299.         while(1)
  300.         {
  301.                 Send(ich1,ich2,ich3,ich4,ich5,ich6);  //串口发送数据                                
  302.                 Delay(1);
  303.                 /*if(key1==0)
  304. ……………………

  306. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
风力摆杆.zip (95.8 KB, 下载次数: 42)


评分

参与人数 1黑币 +100 收起 理由
admin + 100 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:1 发表于 2017-9-6 17:01 | 只看该作者
MPU-6050.C
  1. //========================================================================
  2. //        城建学院电子实验室
  3. //        修改日期2017.07.30
  4. //        版本V1.0
  5. //========================================================================
  6. //特别声明：此程序
  7. //         
  8. //
  9. //
  10. //                                 MCU工作频率:24MHz
  11. //=========================================================================
  12. #include <STC15F2K60S2.h>        //STC15F2K60S2 STC15系列通用头文件
  13. #include <intrins.h>
  14. #include <MPU6050.H>

  16. sbit    SCL=P3^5;                        //IIC时钟引脚定义
  17. sbit    SDA=P3^4;                        //IIC数据引脚定义

  19. typedef unsigned char uchar;
  20. typedef unsigned char uint;       
  21. void  InitMPU6050();                //初始化MPU6050
  22. void  Delay2us();
  23. void  I2C_Start();
  24. void  I2C_Stop();
  25. bit   I2C_RecvACK();
  26. void  I2C_SendByte(uchar dat);
  27. uchar I2C_RecvByte();
  28. void  I2C_ReadPage();
  29. void  I2C_WritePage();
  30. uchar Single_ReadI2C(uchar REG_Address);                                        //读取I2C数据
  31. void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C写入数据
  32. //I^C时序中延时设置，具体参见各芯片的数据手册  6050推荐最小1.3us 但是会出问题，这里延时实际1.9us左右
  33. void Delay2us()       
  34. {
  35.         unsigned char i;
  36.         i = 11;
  37.         while (--i);
  38. }
  39. //**************************************
  40. //I2C起始信号
  41. //**************************************
  42. void I2C_Start()
  43. {
  44.     SDA = 1;                    //拉高数据线
  45.     SCL = 1;                    //拉高时钟线
  46.     Delay2us();                 //延时
  47.     SDA = 0;                    //产生下降沿
  48.     Delay2us();                 //延时
  49.     SCL = 0;                    //拉低时钟线
  50. }
  51. //**************************************
  52. //I2C停止信号
  53. //**************************************
  54. void I2C_Stop()
  55. {
  56.     SDA = 0;                    //拉低数据线
  57.     SCL = 1;                    //拉高时钟线
  58.     Delay2us();                 //延时
  59.     SDA = 1;                    //产生上升沿
  60.     Delay2us();                 //延时
  61. }
  62. //**************************************
  63. //I2C接收应答信号
  64. //**************************************
  65. bit I2C_RecvACK()
  66. {
  67.     SCL = 1;                    //拉高时钟线
  68.     Delay2us();                 //延时
  69.     CY = SDA;                   //读应答信号
  70.     SCL = 0;                    //拉低时钟线
  71.     Delay2us();                 //延时
  72.     return CY;
  73. }
  74. //**************************************
  75. //向I2C总线发送一个字节数据
  76. //**************************************
  77. void I2C_SendByte(uchar dat)
  78. {
  79.     uchar i;
  80.     for (i=0; i<8; i++)         //8位计数器
  81.     {
  82.         dat <<= 1;              //移出数据的最高位
  83.         SDA = CY;               //送数据口
  84.         SCL = 1;                //拉高时钟线
  85.         Delay2us();             //延时
  86.         SCL = 0;                //拉低时钟线
  87.         Delay2us();             //延时
  88.     }
  89.     I2C_RecvACK();
  90. }
  91. //**************************************
  92. //从I2C总线接收一个字节数据
  93. //**************************************
  94. uchar I2C_RecvByte()
  95. {
  96.     uchar i;
  97.     uchar dat = 0;
  98.     SDA = 1;                    //使能内部上拉,准备读取数据,
  99.     for (i=0; i<8; i++)         //8位计数器
  100.     {
  101.         dat <<= 1;
  102.         SCL = 1;                //拉高时钟线
  103.         Delay2us();             //延时
  104.         dat |= SDA;             //读数据
  105.         SCL = 0;                //拉低时钟线
  106.         Delay2us();             //延时
  107.     }
  108.     return dat;
  109. }
  110. //**************************************
  111. //向I2C设备写入一个字节数据
  112. //**************************************
  113. void Single_WriteI2C(uchar REG_Address,uchar REG_data)
  114. {
  115.     I2C_Start();                  //起始信号
  116.     I2C_SendByte(SlaveAddress);   //发送设备地址+写信号
  117.     I2C_SendByte(REG_Address);    //内部寄存器地址，
  118.     I2C_SendByte(REG_data);       //内部寄存器数据，
  119.     I2C_Stop();                   //发送停止信号
  120. }
  121. //**************************************
  122. //从I2C设备读取一个字节数据
  123. //**************************************
  124. uchar Single_ReadI2C(uchar REG_Address)
  125. {
  126.         uchar REG_data;
  127.         I2C_Start();                   //起始信号
  128.         I2C_SendByte(SlaveAddress);    //发送设备地址+写信号
  129.         I2C_SendByte(REG_Address);     //发送存储单元地址，从0开始
  130.         I2C_Start();                   //起始信号
  131.         I2C_SendByte(SlaveAddress+1);  //发送设备地址+读信号
  132.         REG_data=I2C_RecvByte();       //读出寄存器数据
  133.        
  134.         SDA = 1;                    //写应答信号
  135.         SCL = 1;                    //拉高时钟线
  136.         Delay2us();                 //延时
  137.         SCL = 0;                    //拉低时钟线
  138.         Delay2us();                 //延时
  139.        
  140.         I2C_Stop();                    //停止信号
  141.         return REG_data;
  142. }

  144. //**************************************
  145. //初始化MPU6050
  146. //**************************************
  147. void InitMPU6050()
  148. {
  149.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //解除休眠状态
  150.         Single_WriteI2C(SMPLRT_DIV, 0x07);  //陀螺仪125hz
  151.         Single_WriteI2C(CONFIG, 0x04);      //21HZ滤波 延时A8.5ms G8.3ms  此处取值应相当注意，延时与系统周期相近为宜
  152.         Single_WriteI2C(GYRO_CONFIG, 0x08); //陀螺仪500度/S 65.5LSB/g
  153.         Single_WriteI2C(ACCEL_CONFIG, 0x08);//加速度+-4g  8192LSB/g
  154. }
  155. //**************************************
  156. //合成数据
  157. //**************************************
  158. int GetData(uchar REG_Address)
  159. {
  160.         char H,L;
  161.         H=Single_ReadI2C(REG_Address);
  162.         L=Single_ReadI2C(REG_Address+1);
  163.         return (H<<8)+L;   //合成数据
  164. }

  166.        
复制代码
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表