#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空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
