机器人大赛武术擂台赛轻量人型项目比赛

代码片.
#include “Uplib\UP_System.h”
//灰度范围:
//所有数据均以左上角顺时针测量所得
int HDa = 0;//灰度传感器1
int HDb = 0;//灰度传感器2
//红外测距传感器取值范围：1 无障碍；0 有障碍
int HWa = 0;//红外测距传感器1进攻
int HWb = 0;//红外测距传感器2进攻
int HWc = 0;//红外测距传感器3
int HWd = 0;//红外测距传感器4
//头部测距范围>=1600
int THW = 0;//头部测距
//陀螺仪传感器范围
int TLY=0;//陀螺仪传感器
void Init_Data();//初始化数据 Ok
void Start();//开始OK
void Go();//上台OK
void Foot_Down();//下腿关节OK
void Foot_Up();//上腿关节OK
void Get_Up();//上半身直立OK
void Stand_Up();//直立OK
void Fighta();//攻击模式一
void Fightb();//攻击模式二
void Down_Return();//倒下归位OK
void Down_Up_Bk();//倒下模式(后)OK
void Down_Up_Bf();//倒下模式(前)OK
void Common();//正常模式
void Provoke();//挑衅模式
void Turn_Around();//转圈OK
void Turn_Back();//转身搬拦捶后OK
void Turn_Before();//转身搬拦捶前OK
int main(void) {
UP_System_Init();
Start();
while(1) {
Init_Data();
//优先执行倒下模式
if(TLY>=3000) {
Down_Up_Bk();
continue;
} else if(TLY<=1000||(TLY>1000&&TLY<3000&&THW>=1500)) {
Down_Up_Bf();
continue;
}//防御功能 1
if(TLY>=1400&&TLY<=1750&&(HWa0||HWb0)) {
Turn_Before();
Down_Return();
}//防御功能 2
if(TLY>=2400&&TLY<=2800&&(HWa0||HWb0)) {
Turn_Back();
Down_Return();
}
if(((HWa0||HWb0)&&(HDa>=2800||HDb>=2130)&&TLY>1750&&TLY<2400)||((HWc0||HWd0)&&(HDa>=2600||HDb>=1900)&&TLY>1750&&TLY<2400)) { //攻击模式
if(HWc!=0&&HWd!=0) {
Down_Return();
Fighta();
}
if(HWc0||HWd0) {
Down_Return();
Fightb();
}
}
if(HWa!=0&&HWb!=0&&HWc!=0&&HWd!=0&&TLY>1750&&TLY<2400) { //正常模式
Down_Return();
Common();
}
if((HDa>=3150&&HDa<=3160&&TLY>1750&&TLY<2400)||(HDb<=2590&&HDb>=2580&&TLY>1750&&TLY<2400)) { //示威模式
Provoke();
}
}
}
void Init_Data() {
UP_delay_ms(100);
HWa = UP_ADC_GetIO(6);//红外测距传感器数值1
HWb = UP_ADC_GetIO(4);//红外测距传感器数值2
HWc = UP_ADC_GetIO(7);//红外测距传感器数值3
HWd = UP_ADC_GetIO(5);//红外测距传感器数值4
HDa = UP_ADC_GetValue(12);//灰度传感器数值1
HDb = UP_ADC_GetValue(13);//灰度传感器数值2
TLY = UP_ADC_GetValue(15);//陀螺仪倾覆角度数值
THW = UP_ADC_GetValue(8);//头部红外感应
UP_delay_ms(100);
}
void Start() {
while(1) {//开始检测
Stand_Up();
Down_Return();
THW = UP_ADC_GetValue(8);
if(THW>=1500) {//感应到开始命令时开始
UP_CDS_SetMode(1, CDS_SEVMODE);
UP_CDS_SetAngle(1,934,500);
UP_CDS_SetMode(2, CDS_SEVMODE);
UP_CDS_SetAngle(2,934,500);
UP_CDS_SetMode(3, CDS_SEVMODE);
UP_CDS_SetAngle(3,820,400);
UP_CDS_SetMode(4, CDS_SEVMODE);
UP_CDS_SetAngle(4,820,400);
UP_delay_ms(200);
Go();
return ;
}
}
}
void Go() {
//上台开始
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,400);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-400);
//上台时间
UP_delay_ms(1500);
//上台结束
Turn_Around();
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,360);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-350);
UP_delay_ms(1000);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(200);
}
void Foot_Down() {
UP_CDS_SetMode(3, CDS_SEVMODE);
UP_CDS_SetAngle(3,820,780);
UP_CDS_SetMode(4, CDS_SEVMODE);
UP_CDS_SetAngle(4,820,780);
UP_delay_ms(200);
}
void Foot_Up() {
UP_CDS_SetMode(1, CDS_SEVMODE);
UP_CDS_SetAngle(1,934,500);
UP_CDS_SetMode(2, CDS_SEVMODE);
UP_CDS_SetAngle(2,934,500);
UP_delay_ms(200);
}
void Fighta() {//OK
while(1) {
int flag=1;
if(TLY>=2400||TLY<=1750||(TLY>1000&&TLY<3000&&THW>=1500)) {
if(TLY>=1400&&TLY<=1750&&(HWa0||HWb0)) {
Turn_Before();
} else if(TLY>=2400&&TLY<=2800&&(HWa0||HWb0)) {
Turn_Back();
} else if(TLY>1000&&TLY<3000&&THW>=1500)
Down_Up_Bf();
Init_Data();
return ;
}
//四驱
if(HWa!=0||HWb!=0) {
while(1) {
if(HWa0&&HWb!=0&&HDa>=2800&&HDb>=2130&&TLY>1750&&TLY<2400) {
UP_CDS_SetSpeed(11,250);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-450);
UP_delay_ms(10);
} else if(HWb0&&HWa!=0&&HDa>=2800&&HDb>=2130&&TLY>1750&&TLY<2400) {
UP_CDS_SetSpeed(11,450);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-250);
UP_delay_ms(10);
} else if(HWa0&&HWb0&&HDa>=2800&&HDb>=2130&&TLY>1750&&TLY<2400) {
flag=2;
break;
} else if((HWa!=0&&HWb!=0)||TLY<1750||TLY>2400||(HDa<2800||HDb<2130)&&(HWa0||HWb0)||(HDa<=2600||HDb<=1900)) {
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
Init_Data();
return ;
}
Init_Data();
}
}
//右手
if(flag1) {
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,253,1000);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,885,1000);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,800);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,800);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,353,800);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,670,800);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,470,800);
UP_delay_ms(200);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,800);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-800);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,490,1000);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,640,1000);
UP_delay_ms(300);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(200);
} else if(flag2) {
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,425,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,760,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,415,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,600,512);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,512);
UP_delay_ms(200);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,800);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-800);
UP_delay_ms(200);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(200);
}
Init_Data();
}
}
void Fightb() {//OK
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,800);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,800);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,1023,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,831,512);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,340,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,670,512);
UP_delay_ms(200);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,512);
UP_delay_ms(200);
//四驱
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,512,512);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(300);
Init_Data();
}
void Get_Up() {
Foot_Down();
UP_CDS_SetMode(1, CDS_SEVMODE);
UP_CDS_SetAngle(1,512,512);
UP_CDS_SetMode(2, CDS_SEVMODE);
UP_CDS_SetAngle(2,512,512);
UP_delay_ms(200);
}
void Stand_Up() {
UP_CDS_SetMode(1, CDS_SEVMODE);
UP_CDS_SetAngle(1,512,512);
UP_CDS_SetMode(2, CDS_SEVMODE);
UP_CDS_SetAngle(2,512,512);
UP_CDS_SetMode(3, CDS_SEVMODE);
UP_CDS_SetAngle(3,512,512);
UP_CDS_SetMode(4, CDS_SEVMODE);
UP_CDS_SetAngle(4,512,512);
UP_delay_ms(200);
}
void Down_Return() {
//右手
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,599,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,512,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,924,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,76,512);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,512);
UP_delay_ms(200);
}
void Down_Up_Bk() {
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
Down_Return();
Stand_Up();
//上肩
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,937,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,230,512);
//下
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,200,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,810,512);
UP_delay_ms(200);
//中关节
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,512,512);
UP_delay_ms(100);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,512);
UP_delay_ms(100);
Foot_Up();
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,-300);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,300);
Foot_Down();
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,300);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-300);
Down_Return();
//四驱
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
}
void Down_Up_Bf() {
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
Down_Return();
Get_Up();
//上肩
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,255,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,860,512);
UP_delay_ms(100);
//下
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,810,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,205,512);
UP_delay_ms(100);
//中关节
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,512,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,512);
UP_delay_ms(100);
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,512,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,512,512);
UP_delay_ms(100);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,600,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,540,512);
Foot_Up();
Foot_Down();
Down_Return();
}
void Common() {//OK
//右
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,860,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,830,512);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,512,512);
//左手
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,220,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,190,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,110,512);
while(1) {
if(((HWa0||HWb0||(HWc0||HWd0))&&HDa>=2800&&HDb>=2130&&TLY>1750&&TLY<2400)||TLY<=1750||TLY>=2400) {
if(((HWa0||HWb0)&&(HDa>=2800||HDb>=2130)&&TLY>1750&&TLY<2400)||((HWc0||HWd0)&&(HDa>=2600||HDb>=1900)&&TLY>1750&&TLY<2400)) { //攻击模式
if(HWc!=0&&HWd!=0) {
Fighta();
Down_Return();
}
if(HWc0||HWd0) {
Fightb();
Down_Return();
}
}
if(TLY>=2400&&TLY<=2800&&(HWa0||HWb0)) {
Turn_Back();
} else if(TLY>=1400&&TLY<=1750&&(HWa0||HWb0)) {
Turn_Before();
} else if(TLY>3000) {
Down_Up_Bk();
} else if(TLY<=1000||(TLY>1000&&TLY<3000&&THW>=1500)) {
Down_Up_Bf();
}
return ;
} else if((HDa<2800||HDb<2130)&&(HWa0||HWb0)||(HDa<=2600||HDb<=1900)) {
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,-350);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,350);
UP_delay_ms(1000);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,400);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,400);
UP_delay_ms(300);
}
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,350);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-350);
Init_Data();
}
}
void Provoke() {//OK
Down_Return();
int n=2;
while(n!=0) {
if(((HWa0||HWb0)&&HDa>=2800&&HDb>=2130&&TLY>1750&&TLY<2400)||TLY<=1750||TLY>=2400||HDa<2800||HDb<2130) {
return ;
}
if(HDa>=3000&&HDa<=3160) {//模式一
//上
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,302,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,837,512);
UP_delay_ms(100);
//中
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,834,512);
UP_delay_ms(100);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,163,600);
UP_delay_ms(300);
//下
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,600);
UP_delay_ms(300);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,203,512);
UP_delay_ms(300);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,830,600);
UP_delay_ms(300);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,512,600);
UP_delay_ms(200);
Turn_Around();
} else if(HDb<=2592&&HDb>=2500) { //模式二
//右手
UP_CDS_SetMode(5, CDS_SEVMODE);
UP_CDS_SetAngle(5,796,512);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,512);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,860,512);
UP_delay_ms(200);
//左手
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,837,512);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,512,512);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,203,512);
UP_delay_ms(200);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,424,512);
Turn_Around();
}
//四驱
n–;
Init_Data();
}
}
void Turn_Around() {
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,400);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,-200);
UP_delay_ms(350);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
}
void Turn_Back() {
while(HWa0&&HWb0||TLY>=2400&&TLY<=2800&&(HWa0||HWb0)) {
//后侧转身拌拦
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,160,1020);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,700,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,810,512);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,1000,512);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,430,512);
UP_delay_ms(100);
//左捶
UP_CDS_SetSpeed(11,550);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,1000);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,660,1020);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,512);
UP_delay_ms(200);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(200);
Init_Data();
if((TLY<=1000||TLY>=3000)||(HDa<2800&&HDb<2130)||(TLY>1000&&TLY<3000&&THW>=1500))
return ;
}
}
void Turn_Before() {
while(HWa0&&HWb0||TLY>=1400&&TLY<=1750&&(HWa0||HWb0)) {//前侧转身拌拦
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,160,1020);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,700,512);
UP_CDS_SetMode(10, CDS_SEVMODE);
UP_CDS_SetAngle(10,810,512);
UP_CDS_SetMode(7, CDS_SEVMODE);
UP_CDS_SetAngle(7,1000,512);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetMode(8, CDS_SEVMODE);
UP_CDS_SetAngle(8,430,512);
UP_delay_ms(100);
//左捶
UP_CDS_SetSpeed(11,1000);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,550);
UP_CDS_SetMode(9, CDS_SEVMODE);
UP_CDS_SetAngle(9,660,1020);
UP_CDS_SetMode(6, CDS_SEVMODE);
UP_CDS_SetAngle(6,512,512);
UP_delay_ms(200);
UP_CDS_SetMode(11, CDS_MOTOMODE);
UP_CDS_SetSpeed(11,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_CDS_SetMode(12, CDS_MOTOMODE);
UP_CDS_SetSpeed(12,0);
UP_delay_ms(200);
Init_Data();
if((TLY<=1000||TLY>=3000)||(HDa<2800&&HDb<2130)||(TLY>1000&&TLY<3000&&THW>=1500))
return ;
}
}``

生成一个列表

• 轻量级机器人比赛
  • 代码及相关实现