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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › 电子菜鸡
返回列表 发新帖
查看: 842|回复: 2
打印 上一主题 下一主题
收起左侧

电子菜鸡

[复制链接]
跳转到指定楼层
楼主
ID:540851 发表于 2019-5-17 17:33 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
#include<rtx51tny.h>                 /* RTX51 tiny functions & defines */
#include <intrins.h>
#include "LCD12864.h"
#include "key.h"
#include "stc12uart1定时器1.h"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轴的角度，电机的方向通过角度的正负确定
                        {
                                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_PWM0Set(1);
                                HW_PWM1Set(1);
                        }

                        if(mpu6050[6]>-3.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]<9.8&&mpu6050[6]>3.5)
                        {
                                PWM1=1;
                                PWM2=255;        
                        }
                        else
                        {
                                PWM1=1;
                                PWM2=1;
                        }
                }

                if(High01SetValue>=26&&High01SetValue<=27)
                {

                        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(mpu6050[6]>-4.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]<11.0&&mpu6050[6]>3.5)
                        {
                                PWM1=1;
                                PWM2=255;        
                        }
                        else
                        {
                                PWM1=1;
                                PWM2=1;
                        }
                }


                if(High01SetValue>=28&&High01SetValue<=29)
                {
                        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)
                        {
                   )



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

使用道具 举报

沙发
ID:1 发表于 2019-5-18 03:57 | 只看该作者
本帖需要重新编辑补全电路原理图,源码,详细说明与图片即可获得100+黑币(帖子下方有编辑按钮)
回复

使用道具 举报

板凳
ID:56665 发表于 2019-5-18 08:19 | 只看该作者
帖子一定要有详细说明、原理图，只看程序会云里雾里。
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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