51单片机风力摆源程序

ID:223387 · 发表于 2017-7-30 15:13

电子设计大赛往届教程

所有资料51hei提供下载:

风力摆程序.zip (177.13 KB, 下载次数: 83)

2017-7-30 15:13 上传

点击文件名下载附件

单片机源程序如下:

#include<rtx51tny.h> /* RTX51 tiny functions & defines */
#include <intrins.h>
#include "LCD12864.h"
#include "key.h"
#include "stc12uart1定时器1.h"
#include "stc12uart2.h"
#include "HW_PWM.h"
#include<math.h>
#include<stdio.h>
#define uchar unsigned char
#define uint unsigned int
unsigned char Key=0x00; //确定当前哪个按键按下
sbit LED11=P0^2;
sbit LED12=P0^3;
sbit LED13=P0^4;
sbit LED14=P0^7;
/*------------------------------------------------------------------------------------
------------------------------------------------------------------------------------*/
xdata float mpu6050[9];
xdata unsigned char PWM1=1,PWM2=1;
xdata float Gyro_y=0.0;
xdata float Angle_x=0.0; //MPU6050测得的x轴的角度
xdata float Angle_x_set=10; //x轴设定的角度角度与位移的公式 L*tan(Angle_x_set)=x
xdata float x; //L为万向节到纸面原点的垂直距离(单位cm) Angle_x_set为需要设定的角度
xdata float Kp_x=10,Ki_x=0,Kd_x=0.1;
xdata float Now_error_x=0,Last_error_x=0; //Now_error 当前偏差 Last_error 上一次偏差 Pre_error上上一次偏差
xdata int uk_x=0,uk1_x=0.01,duk_x=0;
xdata float Kp_y=10,Ki_y=0,Kd_y=0.1;
xdata float Now_error_y=0,Last_error_y=0; //Now_error 当前偏差 Last_error 上一次偏差 Pre_error上上一次偏差
xdata int uk_y=0,uk1_y=0,duk_y=0;
xdata float setjiaodu3=0;
uchar m,i;
bit Basic04Flag=0; //基础部分4标志位
//bit Basic04Flag2=0;
//x为纸面的位移
int PID_control();
/*------------------------------------------------------------------------------------
------------------------------------------------------------------------------------*/
void SystemInit(void) _task_ 0 //任务0----------------系统初始化
{
LCD12864_Init(); //12864液晶初始化
KeyInit(); //按键初始化
UART2_Init(); //STC12C5A串口2初始化，独立波特率发生器产生波特率
UART1_Init(); //STC12C5A串口1初始化，定时器1产生波特率
HW_PWMInit();
HW_PWM0Set(1);
HW_PWM1Set(1);
PWM1=1;
PWM2=1;
MenuOperate(0);
MenuOperate(0);
MenuOperate(0);
LED11=1;
LED12=1;
LED13=1;
LED14=1;
os_create_task(1); //创建任务1
os_create_task(2); //创建任务2
os_create_task(3); //创建任务3
os_create_task(4); //创建任务4
os_create_task(5); //创建任务5
os_create_task(6); //创建任务6
os_create_task(7); //创建任务7
os_create_task(8); //创建任务8
os_create_task(9); //创建任务9
os_delete_task(0); //删除初始化函数
}
void KeyScan(void) _task_ 1 //任务1-----------------------按键扫描
{
unsigned char keyvalue=KEY_NULL;
while(1)
{
GetKey(&keyvalue); //获取当前键值
if(keyvalue==(KEY_VALUE_1|KEY_DOWN)) //按键一按下指针减
{
Key=0x30;
os_send_signal(2);
}
if(keyvalue==(KEY_VALUE_2|KEY_DOWN)) //按键er按下指针加
{
Key=0x28;
os_send_signal(2);
}
if(keyvalue==(KEY_VALUE_3|KEY_DOWN)) //按键san按下确定
{
Key=0x18;
os_send_signal(2);
}
if(keyvalue==(KEY_VALUE_4|KEY_DOWN)) //按键四按下返回
{
Key=0x08;
os_send_signal(2);
}
os_wait(K_IVL,3,0); //进行周期性检测
}
}
void LCD12864Display(void) _task_ 2 //任务2-----------------12864液晶显示
{
while(1)
{
os_wait(K_SIG,0,0); //等待按键信号
MenuOperate(Key);
}
}
void UartRec(void) _task_ 3 //任务3-------------------串口收发
{
// float Value[3];
// float x,y,z;
// xdata char str[16];
while(1)
{
// Value [0] = ((short)(ucStrAngle[1]<<8| ucStrAngle[0]))/32768.0*180;
// Value[1] = ((short)(ucStrAngle[3]<<8| ucStrAngle[2]))/32768.0*180;
// Value[2] = ((short)(ucStrAngle[5]<<8| ucStrAngle[4]))/32768.0*180;
// acc_x=(float)(((unsigned short)ucStra[3]<<8)|ucStra[2])/32768.0*16;
Angle_x=((short)(ucStrAngle[3]<<8| ucStrAngle[2]))/32768.0*180;
Gyro_y=((short)(ucStrw[3]<<8|ucStrw[2]))/32768.0*2000;
// mpu6050[0]=(float)mpu[0]/32768.0*16; //加速度
// mpu6050[1]=(float)mpu[1]/32768.0*16;
// mpu6050[2]=(float)mpu[2]/32768.0*16;
mpu6050[3]=(float)mpu[3]/32768.0*2000; //角速度
mpu6050[4]=(float)mpu[4]/32768.0*2000;
mpu6050[5]=(float)mpu[5]/32768.0*2000;
mpu6050[6]=(float)mpu[6]/32768.0*180; //角度
mpu6050[7]=(float)mpu[7]/32768.0*180;
mpu6050[8]=(float)mpu[8]/32768.0*180;
// sprintf(str,"%.3f %.3f\n",mpu6050[6],mpu6050[7]);
// UART1_SendStr(str);
UART1_SendPWM(PWM1,PWM2);
// Data_Send_Status(Value[0],Value[1],Value[2]);
// Data_Receive(&x,&y,&z);
// UART1_SendPWM(64,64);
os_switch_task();
}
}
void TaskSwitch(void) _task_ 4 //任务4
{
while(1)
{
switch(KeyFuncIndex)
{
case 10: //基础1
// HW_PWM0Set(1);
// HW_PWM1Set(1);
os_send_signal(5);
break;
case 11: //基础2
//HW_PWM0Set(1);
//HW_PWM1Set(1);
os_send_signal(6);
break;
case 12: //基础3
os_send_signal(7);
break;
case 16: //基础4
os_send_signal(9);
break;
case 13: //发挥1
os_send_signal(8);
break;
case 14: //发挥2
os_send_signal(9);
break;
case 15: //发挥3
break;
}
os_switch_task();
}
}
void PID_control_x()
{
Now_error_x=-mpu6050[6]; //偏差=设定值—实际值
duk_x=Kp_x*(Now_error_x-Last_error_x)+Ki_x*Now_error_x+Kd_x*mpu6050[3]; //pid增量式公式
uk_x=duk_x+uk1_x; //通过pid调节所需要改变的值
uk1_x=uk_x; //变量值移位
Last_error_x=Now_error_x;
}
void PID_control_y()
{
Now_error_y=0.0-mpu6050[7]; //偏差=设定值—实际值
duk_y=Kp_y*(Now_error_y-Last_error_y)+Ki_y*Now_error_y+Kd_y*mpu6050[4]; //pid增量式公式
uk_y=duk_y+uk1_y; //通过pid调节所需要改变的值
uk1_y=uk_y; //变量值移位
Last_error_y=Now_error_y;
}
void Basic01(void) _task_ 5 //任务5 基础部分1
{
while(1)
{
os_wait(K_SIG,0,0);
// PID_control_x();
if(Angle_x>-3.0&&Angle_x<10.5) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-3.0&&Angle_x>-7.5)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-7.5&&Angle_x>-9.8)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
// if(mpu6050[6]>2)
// {
// uk_x=fabs(uk_x);
// if(uk_x>=256)
// {
// uk_x=255;
// }
// PWM1=1;
// PWM2=uk_x;
// //hou yiwozhengqianfangweizuobuao
// // LED14=1;
// }
// if(mpu6050[6]<-2)
// {
// uk_x=fabs(uk_x);
// if(uk_x>=256)
// {
// uk_x=255;
// }
// PWM1=uk_x;
// PWM2=1;
//
// }
// else
// {
// PWM1=1;
// PWM2=1;
// }
// HW_PWM1Set(1);
// HW_PWM0Set(255);
// os_wait(K_TMO,122,0);
// HW_PWM0Set(1);
// HW_PWM1Set(155);
// os_wait(K_TMO,38,0);
os_switch_task();
}
}
void Basic02(void) _task_ 6 //任务6 基础部分2
{
// xdata char str[16];
HW_PWM0Set(1);
HW_PWM1Set(1);
while(1)
{
os_wait(K_SIG,0,0);
Angle_x_set=atan(Basic02SetValue/2.0/92.0)*57.295;
// sprintf(str,"%.3f %.3f\n",Angle_x,Gyro_y);
// UART1_SendStr(str);
if(Basic02SetValue>=30&&Basic02SetValue<=32)
{
if(Angle_x>-1.0&&Angle_x<3.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<0.0&&Angle_x>-3.0)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-3.0&&Angle_x>-5.5)
{
HW_PWM0Set(1);
HW_PWM1Set(245);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=33&&Basic02SetValue<=35)
{
if(Angle_x>-1.0&&Angle_x<3.3) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-1.0&&Angle_x>-3.3)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-3.3&&Angle_x>-5.5)
{
HW_PWM0Set(1);
HW_PWM1Set(245);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue==36) //wucha
{
if(Angle_x>-1.2&&Angle_x<3.5) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-1.0&&Angle_x>-3.5)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-3.5&&Angle_x>-5.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=37&&Basic02SetValue<=39)
{
if(Angle_x>-2.0&&Angle_x<3.5) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-3.5)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-3.5&&Angle_x>-6.0)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=40&&Basic02SetValue<=43)
{
if(Angle_x>-2.0&&Angle_x<4.9) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-4.9)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-4.9&&Angle_x>-7.3)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue==44)
{
if(Angle_x>-2.0&&Angle_x<4.7) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-4.7)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-4.7&&Angle_x>-7.2)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=45&&Basic02SetValue<=46)
{
if(Angle_x>-2.0&&Angle_x<5.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-4.9)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-4.9&&Angle_x>-7.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=47&&Basic02SetValue<=49)
{
if(Angle_x>-2.0&&Angle_x<5.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-6.0)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-6.0&&Angle_x>-8.6)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=50&&Basic02SetValue<=51)
{
if(Angle_x>-2.0&&Angle_x<6.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-6.9)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-6.9&&Angle_x>-9.8)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=52&&Basic02SetValue<=53)
{
if(Angle_x>-2.0&&Angle_x<5.6) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-7.6)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-7.6&&Angle_x>-10.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=54&&Basic02SetValue<=55)
{
if(Angle_x>-2.0&&Angle_x<6.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-8.0)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-8.0&&Angle_x>-11.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=56&&Basic02SetValue<=58)
{
if(Angle_x>-2.0&&Angle_x<7.3) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-7.3)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-7.3&&Angle_x>-11.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic02SetValue>=59&&Basic02SetValue<=60)
{
if(Angle_x>-2.0&&Angle_x<7.8) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-7.8)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-7.8&&Angle_x>-12.5)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
os_switch_task();
}
}
void Basic03(void) _task_ 7 //任务7 基础部分3
{
while(1)
{
os_wait(K_SIG,0,0);
if(Basic03SetValue==10) //10度
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(55);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=55;
}
else
{
PWM1=205;
PWM2=1;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==20) //20度
{
if(mpu6050[3]>0)
{
HW_PWM0Set(75);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=255;
}
else
{
PWM1=255;
PWM2=1;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==30)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(90);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
if(mpu6050[7]>15)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=255;
}
else
{
PWM1=255;
PWM2=1;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==40)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(120);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
if(mpu6050[7]>15)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=255;
}
else
{
PWM1=255;
PWM2=1;
}
if(mpu6050[6]>15)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==50)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(105);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(168);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=205;
}
else
{
PWM1=205;
PWM2=1;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==60)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(205);
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=205;
}
else
{
PWM1=125; //145
PWM2=1;
}
if(mpu6050[6]>20)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==70)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(145); //145
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(145);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=205;
}
else
{
PWM1=1; //145
PWM2=1;
}
if(mpu6050[6]>20)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==80)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(145); //145
HW_PWM1Set(1);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(145);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=1;
PWM2=205;
}
else
{
PWM1=1; //145
PWM2=1;
}
if(mpu6050[6]>20)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==90)
{
if(Angle_x>-3.0&&Angle_x<10.5) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-3.0&&Angle_x>-7.5)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-7.5&&Angle_x>-9.8)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==100)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(210);
}
else
{
HW_PWM0Set(175);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=45;
PWM2=1;
}
else
{
PWM1=1;
PWM2=45;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==110)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
else
{
HW_PWM0Set(175);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=95;
PWM2=1;
}
else
{
PWM1=1;
PWM2=95;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==120)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(185);
}
else
{
HW_PWM0Set(175);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=155;
PWM2=80;
}
else
{
PWM1=1;
PWM2=105;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==130)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(205);
}
else
{
HW_PWM0Set(175);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=195; //185
PWM2=80;
}
else
{
PWM1=1;
PWM2=95;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==140)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(165);
}
else
{
HW_PWM0Set(155);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=205;
PWM2=1;
}
else
{
PWM1=1;
PWM2=105;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==150)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(225);
}
else
{
HW_PWM0Set(35);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=185;
PWM2=1;
}
else
{
PWM1=1;
PWM2=185;
}
if(mpu6050[6]>15)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==160)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(185);
}
else
{
HW_PWM0Set(35);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=205;
PWM2=1;
}
else
{
PWM1=1;
PWM2=205;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
if(Basic03SetValue==170)
{
if(mpu6050[3]>0)
{
HW_PWM0Set(1);
HW_PWM1Set(95);
}
else
{
HW_PWM0Set(5);
HW_PWM1Set(1);
}
if(mpu6050[7]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
/*处理Y轴*/
if(mpu6050[4]>0)
{
PWM1=205;
PWM2=1;
}
else
{
PWM1=1;
PWM2=205;
}
if(mpu6050[6]>30)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
}
// mpu6050[5]=(float)mpu[5]/32768.0*2000;
// if(mpu6050[3]<0&&mpu6050[4]>0)
// {
// LED11=0; //hou yiwozhengqianfangweizuobuao
// LED14=1;
// if(mpu6050[6]<3.0&&mpu6050[6]>0)
// {
// PWM1=1;
// PWM2=255;
// }
//
// }
// if(mpu6050[3]<0&&mpu6050[4]<0)
// {
// LED12=0; //zuo
// LED13=1;
// HW_PWM0Set(uk);
// }
// if(mpu6050[3]>0&&mpu6050[4]>0)
// {
// LED13=0; //有
// LED12=1;
// }
// if(mpu6050[3]>0&&mpu6050[4]<0)
// {
// LED14=0;//qian
// LED11=1;
// if(mpu6050[6]<0&&mpu6050[6]>-3.0)
// {
// PWM1=255;
// PWM2=1;
// }
// }
os_switch_task();
}
}
void High01(void) _task_ 8 //任务8 发挥1 High01SetValue
{
PWM1=1;
PWM2=1;
HW_PWM0Set(1);
HW_PWM1Set(1);
while(1)
{
os_wait(K_SIG,0,0);
if(High01SetValue==15)
{
if(mpu6050[7]>-1.0&&mpu6050[7]<3.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(mpu6050[7]<0.0&&mpu6050[7]>-3.0)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(mpu6050[7]<-3.0&&mpu6050[7]>-5.5)
{
HW_PWM0Set(1);
HW_PWM1Set(245);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
if(mpu6050[6]>-1.0&&mpu6050[6]<3.2) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
PWM1=255;
PWM2=1;
}
else if(mpu6050[6]<-1.0&&mpu6050[6]>-5.0)
{
PWM1=1;
PWM2=1;
}
else if(mpu6050[6]<6.0&&mpu6050[6]>3.2)
{
PWM1=1;
PWM2=245;
}
else
{
PWM1=1;
PWM2=1;
}
}
if(High01SetValue>=16&&High01SetValue<=18)
{
if(Angle_x>-2.0&&Angle_x<4.9) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
HW_PWM0Set(255);
HW_PWM1Set(1);
}
else if(Angle_x<-2.0&&Angle_x>-4.9)
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
else if(Angle_x<-4.9&&Angle_x>-7.3)
{
HW_PWM0Set(1);
HW_PWM1Set(255);
}
else
{
HW_PWM0Set(1);
HW_PWM1Set(1);
}
if(mpu6050[6]>-1.8&&mpu6050[6]<3.5) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
PWM1=255;
PWM2=1;
}
else if(mpu6050[6]<-2.0&&mpu6050[6]>-5.0)
{
PWM1=1;
PWM2=1;
}
else if(mpu6050[6]<7.0&&mpu6050[6]>3.5)
{
PWM1=1;
PWM2=255;
}
else
{
PWM1=1;
PWM2=1;
}
}
if(High01SetValue>=18&&High01SetValue<=19)
{
if(Angle_x>-2.0&&Angle_x<6.0) //Angle_x为x轴的角度，电机的方向通过角度的正负确定
{
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码

ID:225587 · 发表于 2017-8-7 20:23

哥，没钱啊我

ID:225587 · 发表于 2017-8-7 20:24

能不能发我邮箱，谢谢哥，1282388187@qq.com

ID:105206 · 发表于 2018-6-12 13:33

谢谢分享，学习了

ID:257454 · 发表于 2018-7-15 20:07

好东西

帐号		自动登录	找回密码
密码			立即注册