机械结构如图所示,上面有标注AD号,就是接收不到传感器的模拟值,但是软开关使用没有问题,证明传感器是好的,只是在开始的时候能用一下,但是后面传感器的值好像都是0。求大佬指一下名路,调几天了,还是没有头绪,编译环境用的是keil5,。以下附上代码。
#include"UPLib\UP_System.h"
int AD0=0; //头部红外测距传感器
int AD1=0; //灰度传感器
int AD2=0; //后左红外测距传感器
int AD3=0; //后红外测距传感器
int AD4=0; //前斜下红外测距传感器
int AD5=0; //右红外测距传感器
int AD6=0; //前红外测距传感器
int AD7=0; //倾角红外测距传感器
int AD8=0; //前右红外测距传感器
int nh=0;
int nq=0;
int k=0;
int r=0;
int m=0;
int nLastLeft=0;
int nLastRight=0;
int nStand=0;
int nEdge=0;
void Move(int inLeft, int inRight)//定义移动
{
UP_CDS_SetSpeed(12, -inLeft*0.6);
UP_CDS_SetSpeed(11, inRight*0.6);
//MFServoAction();
nLastLeft = inLeft;
nLastRight = inRight;
}
unsigned char SStand() //检测机器人是否倒地
{ int AD7=0;
AD7 =UP_ADC_GetValue(7);
if((AD7>1712)&&(AD7<2512))
{
return 1; //stand up
}
else if (AD7<1312)
{
return 2; //qiandao
}
else if (AD7>2912)
{
return 3; //houdao
}
else
{
return 0; //zhengchang
}
}
unsigned char Edge()
{
int AD1 = 0; //huidu diastimeter
int AD2 = 0; //back left diastimeter
int AD3 = 0; //back diastimeter
int AD4 = 0; //back down diastimeter
int AD5 = 0; //right diastimeter
int AD6 = 0; //forward diastimeter
int AD7 = 0; //MC-8 incline
int AD8 = 0; //rightforward diastimeter
AD1=UP_ADC_GetValue(1);
AD2=UP_ADC_GetValue(2);
AD3=UP_ADC_GetValue(3);
AD4=UP_ADC_GetValue(4);
AD5=UP_ADC_GetValue(5);
AD6=UP_ADC_GetValue(6);
AD7=UP_ADC_GetValue(7);
AD8=UP_ADC_GetValue(8);
//if((AD4400))
if((AD4>700)&&(AD1>400))// no margin
{
AD1=UP_ADC_GetValue(1);
AD2=UP_ADC_GetValue(2);
AD3=UP_ADC_GetValue(3);
AD4=UP_ADC_GetValue(4);
AD5=UP_ADC_GetValue(5);
AD6=UP_ADC_GetValue(6);
AD7=UP_ADC_GetValue(7);
AD8=UP_ADC_GetValue(8);
if((AD1>500)||(AD4>400))//forward no opponent
{
return 1;
}
else if(AD5
{
return 1;
}
else if(AD3
{
return 1;
}
else if(AD2
{
return 1;
}
else if(AD8>600)//forward opponent
{
return 7;
}
else if(AD5>800)//right opponent
{
return 4;
}
else if((AD2>600)&&(AD3>800))//back opponent
{
return 6;
}
else if(AD2>800)//left opponent
{
return 5;
}
else
{
return 1;
}
}
// {
// return 1;
// }
// else if((AD5700)||(AD1>500)))//left no opponent and no margin
// {
// return 1;
// }
// else if((AD3700)||(AD1>500)))//back no opponent and no margin
// {
// return 1;
// }
// else if((AD2700)||(AD1>500)))//leftback no opponent and no margin
// {
// return 1;
// }
else if((AD1
{
if((AD6
{
return 2;
}
else
{
return 0;
}
}
// else if(AD8>600)//forward opponent
// {
// return 7;
// }
// else if(AD5>800)//right opponent
// {
// return 4;
// }
// else if((AD2>600)||(AD3>800))//back opponent
// {
// return 6;
// }
// else if((AD2>800)||(AD8>800))//left opponent
// {
// return 5;
// }
else if((AD4>800)&&(AD1<500))//forward margin
{
return 2;
}
else
{
return 0;//normal
}
}
void DefaultAction() //stand up
{
UP_CDS_SetAngle(1, 480, 400);
UP_CDS_SetAngle(2, 250, 400);
UP_CDS_SetAngle(3, 325, 400);
UP_CDS_SetAngle(4, 527, 400);
UP_CDS_SetAngle(5, 522, 400);
UP_CDS_SetAngle(6, 541, 400);
UP_CDS_SetAngle(7, 560, 400);
UP_CDS_SetAngle(8, 705, 400);
UP_CDS_SetAngle(9, 210, 400);
UP_CDS_SetAngle(10, 529, 400);
//MFServoAction();
}
void Sit()//sit down
{
UP_CDS_SetAngle(1, 812, 400);
UP_CDS_SetAngle(2, 323, 400);
UP_CDS_SetAngle(3, 695, 400);
UP_CDS_SetAngle(4, 82, 400);
UP_CDS_SetAngle(5, 182, 400);
UP_CDS_SetAngle(6, 876, 400);
UP_CDS_SetAngle(7, 997, 400);
UP_CDS_SetAngle(8, 339, 400);
UP_CDS_SetAngle(9, 284, 400);
UP_CDS_SetAngle(10,874, 400);
}
void StandA()//qiandao
{
UP_CDS_SetAngle(1, 865, 512);
UP_CDS_SetAngle(2, 287, 512);
UP_CDS_SetAngle(3, 711, 512);
UP_CDS_SetAngle(8, 345, 512);
UP_CDS_SetAngle(9, 294 ,512);
UP_CDS_SetAngle(10,876, 512);
UP_CDS_SetAngle(4, 84, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 867, 512);
UP_CDS_SetAngle(7, 992, 512); //?????(????)?????
UP_delay_ms(500);
UP_CDS_SetAngle(1, 690, 512);
UP_CDS_SetAngle(2, 431, 1000);
UP_CDS_SetAngle(3, 469, 1000);
UP_CDS_SetAngle(8, 345, 512);
UP_CDS_SetAngle(9, 415,1000);
UP_CDS_SetAngle(10, 481,1000);//??????,????????
UP_delay_ms(1000);
Move(-100,-5100);//??????
UP_CDS_SetAngle(1, 865, 512);
UP_CDS_SetAngle(2, 287, 512);
UP_CDS_SetAngle(3, 711, 512);
UP_CDS_SetAngle(8, 345, 512);
UP_CDS_SetAngle(9, 294 ,512);
UP_CDS_SetAngle(10, 876, 512); //??????
}
void StandB()//houdao
{
UP_CDS_SetAngle(1, 865, 512);
UP_CDS_SetAngle(2, 287, 512);
UP_CDS_SetAngle(3, 711, 512);
//?????(????)?????
UP_CDS_SetAngle(4, 84, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 867, 512);
UP_CDS_SetAngle(7, 992, 512); //?????(????)?????
UP_CDS_SetAngle(8, 345, 512);
UP_CDS_SetAngle(9, 294 ,512);
UP_CDS_SetAngle(10, 876, 512);
UP_delay_ms(500);
UP_CDS_SetAngle(1,789, 512);
UP_CDS_SetAngle(2, 930, 512);
UP_CDS_SetAngle(9, 848, 512);
UP_CDS_SetAngle(10,876, 512);//?????????
UP_delay_ms(1000);
UP_CDS_SetAngle(3, 64, 512);
UP_CDS_SetAngle(8, 944, 512);//??????
UP_delay_ms(500);
UP_CDS_SetAngle(1, 699, 512);
UP_CDS_SetAngle(2, 461, 512);
UP_CDS_SetAngle(9,417, 512);
UP_CDS_SetAngle(10,754, 512);
//?????????
UP_delay_ms(1500);
UP_CDS_SetAngle(4, 216, 512);
UP_CDS_SetAngle(7, 862, 512);//?????
UP_delay_ms(500);
UP_CDS_SetAngle(4,457, 512);
UP_CDS_SetAngle(12, 616, 512);//?????????
UP_delay_ms(1000);
UP_CDS_SetAngle(4, 84, 512);
UP_CDS_SetAngle(7,962, 512);//??
UP_delay_ms(1000);
UP_CDS_SetAngle(1, 865, 812);
UP_CDS_SetAngle(2, 287, 512);
UP_CDS_SetAngle(3, 711, 512);
UP_CDS_SetAngle(8, 345, 812);
UP_CDS_SetAngle(9, 294, 512);
UP_CDS_SetAngle(10, 876, 512);//???
UP_delay_ms(1000);
Move(-100,-5100);//??????
}
void Attack1()//attack left opponent
{
UP_CDS_SetAngle(1, 843, 512);
UP_CDS_SetAngle(2, 288, 512);
UP_CDS_SetAngle(3, 712, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 518, 512);
UP_CDS_SetAngle(9, 560, 512);
UP_CDS_SetAngle(10, 512, 512);
}
void Attack2()//attack right opponent
{
UP_CDS_SetAngle(1, 490, 512);
UP_CDS_SetAngle(2, 526, 512);
UP_CDS_SetAngle(3, 458, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 339, 512);
UP_CDS_SetAngle(9, 273, 512);
UP_CDS_SetAngle(10, 892, 512);
}
void Attack3()//attack forword opponent
{
UP_CDS_SetAngle(1, 490, 512);
UP_CDS_SetAngle(2, 664, 512);
UP_CDS_SetAngle(3, 718, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 310, 512);
UP_CDS_SetAngle(9, 627, 512);
UP_CDS_SetAngle(10, 550, 512);
}
void Attack4()//attack back opponent
{
UP_CDS_SetAngle(1, 519, 512);
UP_CDS_SetAngle(2, 501, 512);
UP_CDS_SetAngle(3, 48, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 991, 512);
UP_CDS_SetAngle(9, 490, 512);
UP_CDS_SetAngle(10, 547, 512);
}
void Attack5()//attack intersect 1
{
UP_CDS_SetAngle(1, 831, 512);
UP_CDS_SetAngle(2, 291, 512);
UP_CDS_SetAngle(3, 712, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 354, 512);
UP_CDS_SetAngle(9, 634, 512);
UP_CDS_SetAngle(10, 559, 512);
}
void Attack6()//attack intersect 2
{
UP_CDS_SetAngle(1, 525, 512);
UP_CDS_SetAngle(2, 667, 512);
UP_CDS_SetAngle(3, 731, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 324, 512);
UP_CDS_SetAngle(9, 294, 512);
UP_CDS_SetAngle(10, 900, 512);
}
void Attack7()//attack back opponent
{
UP_CDS_SetAngle(1, 949, 512);
UP_CDS_SetAngle(2, 224, 512);
UP_CDS_SetAngle(3, 55, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 966, 512);
UP_CDS_SetAngle(9, 481, 512);
UP_CDS_SetAngle(10, 551, 512);
UP_delay_ms(500);
UP_CDS_SetAngle(1, 500, 512);
UP_CDS_SetAngle(2, 511, 512);
UP_CDS_SetAngle(3, 56, 512);
UP_CDS_SetAngle(4, 82, 512);
UP_CDS_SetAngle(5, 180, 512);
UP_CDS_SetAngle(6, 870, 512);
UP_CDS_SetAngle(7, 1000, 512);
UP_CDS_SetAngle(8, 984, 512);
UP_CDS_SetAngle(9, 176, 512);
UP_CDS_SetAngle(10, 900, 512);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int main(void)
{
UP_System_Init(); //3?ê??¯
// MFSetPortDirect(0x0f00); //éè??IO·??ò
// DelayMS(3000); //?óê±3??
// MFInitServoMapping(&SERVO_MAPPING[0], 12);
UP_CDS_SetMode(11,CDS_MOTOMODE); //定义舵机模式(舵机模式和电机模式)
UP_CDS_SetMode(12,CDS_MOTOMODE);
UP_CDS_SetMode(3,CDS_SEVMODE);
UP_CDS_SetMode(4,CDS_SEVMODE);
UP_CDS_SetMode(5,CDS_SEVMODE);
UP_CDS_SetMode(6,CDS_SEVMODE);
UP_CDS_SetMode(7,CDS_SEVMODE);
UP_CDS_SetMode(8,CDS_SEVMODE);
UP_CDS_SetMode(1,CDS_SEVMODE);
UP_CDS_SetMode(2,CDS_SEVMODE);
UP_delay_ms(100);
//int nStand = 0;
//int nEdge = 0;
DefaultAction(); //初始动作
UP_delay_ms(800);
//MFServoAction();
while (1)//软开关,开启机器人
{
AD5=UP_ADC_GetValue(5);
if (AD5>1200)
{Sit();
UP_delay_ms(800);
break;
}
UP_delay_ms(50);
}
Move(700, 800);//dengtai
UP_delay_ms(1400);
Move(600, 800);
UP_delay_ms(1500);
while (1)
{
nStand=SStand();//?ì2a×?éí×'ì?
switch (nStand)
{
case 1: //
nEdge=Edge();//?ì2a×?éí????
switch (nEdge)
{
case 1: //?TµDè??T±??µ
k++;
if(k>350)
{
Move(500, -500);
UP_delay_ms(50);
k = 0;
}
else
{
Move(500,500);
UP_delay_ms(10);
}
Sit();
break;
case 2: //
r++;
if(r<4)
{
Move(350,350);
UP_delay_ms(50);
Move(600,-600);
UP_delay_ms(600);
Move(0,0);
UP_delay_ms(10);
Sit();
UP_delay_ms(10);
}
else if((r>3)&&(r<9))
{
Move(350, 350);
UP_delay_ms(500);
Move(-600, 600);
UP_delay_ms(300);
Move(0, 0);
UP_delay_ms(10);
Sit();
UP_delay_ms(50);
}
else if(r>12)
{
r=0;
}
else
{
Move(350, 350);
UP_delay_ms(500);
Move(700, -700);
UP_delay_ms(500);
Move(0, 0);
UP_delay_ms(10);
Sit();
UP_delay_ms(50);
}
// if((AD1<800)&&(AD4<800))
// {
// Move(-500,500);
// UP_delay_ms(500);
// Move(800,800);
// UP_delay_ms(800);
// Attack2();
// UP_delay_ms(100);
// }
// else if((810<AD1)&&(AD1<1000))
// {
// Move(-500,500);
// UP_delay_ms(500);
// Attack1();
// UP_delay_ms(100);
// }
// else
// {
// Move(800,800);
// UP_delay_ms(200);
// Sit();
// UP_delay_ms(10);
// }
//
break;
case 4: //right opponent
AD1=UP_ADC_GetValue(1);
AD3=UP_ADC_GetValue(3);
AD6=UP_ADC_GetValue(6);
AD8=UP_ADC_GetValue(8);
AD5=UP_ADC_GetValue(5);
Move(600,-600);
UP_delay_ms(1000);
if((AD5>500)||((AD6>320)&&(AD5>320)))
{
UP_delay_ms(10);
Attack1();
UP_delay_ms(400);
Attack3();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Move(-500,-500);
Sit();
UP_delay_ms(50);
}
else
{
Move(-300, -300);
UP_delay_ms(200);
Move(400, -400);
UP_delay_ms(200);
Sit();
UP_delay_ms(50);
}
break;
case 5: //left opponent
Move(-700, 700);
UP_delay_ms(500);
Move(-400, -400);
UP_delay_ms(350);
Move(0, 0);
UP_delay_ms(10);
Sit();
UP_delay_ms(50);
AD2=UP_ADC_GetValue(2);
AD8=UP_ADC_GetValue(8);
if((AD2>400)||(AD8>320))
{
Attack2();
UP_delay_ms(400);
Attack3();
UP_delay_ms(400);
Move(800, 800);
UP_delay_ms(10);
Attack2();
UP_delay_ms(400);
Move(0, 0);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(400);
Attack5();
UP_delay_ms(400);
Attack6();
UP_delay_ms(50);
Sit();
UP_delay_ms(50);
}
else
{
Move(-300, -300);
UP_delay_ms(200);
Move(400, -400);
UP_delay_ms(200);
Sit();
UP_delay_ms(50);
}
break;
case 7:
m++;
Move(-500, -400);
UP_delay_ms(900);
Sit(); //ready attack
UP_delay_ms(100);
Move(500, 500);
UP_delay_ms(300);
Move(300, 300);
UP_delay_ms(500);
Sit();
UP_delay_ms(50);
AD8=UP_ADC_GetValue(8);
AD4=UP_ADC_GetValue(4);
AD6=UP_ADC_GetValue(6);
if(((AD4>800)||(AD6>320)||(AD8>480))&&(m==1))
/////////////if((AD6>320)||(AD3>480)))&&(m==1))
{
Move(300, 300);
UP_delay_ms(50);
Sit();
UP_delay_ms(500);
Move(300,300);
UP_delay_ms(100);
Attack3();
UP_delay_ms(1000);
Attack2();
UP_delay_ms(400);
Attack5();
UP_delay_ms(500);
Attack6();
UP_delay_ms(500);
Attack5();
UP_delay_ms(500);
Attack6();
UP_delay_ms(500);
Attack5();
UP_delay_ms(500);
Attack6();
UP_delay_ms(500);
Move(-500, -400);
UP_delay_ms(800);
Move(0, 0);
UP_delay_ms(10);
Sit();
UP_delay_ms(50);
m=0;
}
else
{
Move(100, 100);
UP_delay_ms(200);
Move(0, 0);
UP_delay_ms(200);
Move(-300, -300);
UP_delay_ms(200);
Move(400, -400);
UP_delay_ms(200);
Sit();
UP_delay_ms(50);
m=0;
}
break;
case 6:
Move(600,600);
UP_delay_ms(300);
Move(400,400);
UP_delay_ms(200);
UP_CDS_SetAngle(1, 949, 512);
UP_CDS_SetAngle(2, 244, 512);
UP_CDS_SetAngle(3, 55, 512);
UP_CDS_SetAngle(8, 996, 512);
UP_CDS_SetAngle(9, 176, 512);
UP_CDS_SetAngle(10, 989, 512);
Move(-400,-400);
UP_delay_ms(400);
Attack7();
UP_delay_ms(1000);
Attack7();
UP_delay_ms(1000);
Attack7();
UP_delay_ms(1000);
Attack7();
UP_delay_ms(1000);
Move(-500,-500);
UP_delay_ms(300);
Move(-300,-300);
UP_delay_ms(200);
Move(400,400);
UP_delay_ms(800);
Sit();
UP_delay_ms(500);
break;
}
break;
case 2:
nq++;
if(nq==6)
{
Move(0, 0);
UP_delay_ms(100);
Sit();
UP_delay_ms(800);
StandA();
UP_delay_ms(2000);
Sit();
UP_delay_ms(50);
nq=0;
}
else
{
UP_delay_ms(40);
}
break;
case 3: //houdao
nh++;
if (nh==6)
{
Move(0, 0);
UP_delay_ms(100);
Sit();
UP_delay_ms(800);
StandB();
UP_delay_ms(2000);
Sit();
UP_delay_ms(50);
nh=0;
}
else
{
UP_delay_ms(50);
}
break;
case 0:
Move(400,400);
UP_delay_ms(400);
Move(200,300);
UP_delay_ms(200);
Move(0,0);
UP_delay_ms(10);
Sit();
Attack2();
UP_delay_ms(100);
if(AD1>800)
{
Move(-500,500);
UP_delay_ms(400);
Move(800,800);
UP_delay_ms(300);
Move(-400,400);
UP_delay_ms(500);
Move(0,0);
UP_delay_ms(50);
Sit();
Attack1();
UP_delay_ms(100);
}
else if(AD1<800)
{
Move(800,700);
UP_delay_ms(400);
Attack5();
UP_delay_ms(200);
Attack6();
UP_delay_ms(200);
Attack5();
UP_delay_ms(200);
Attack6();
UP_delay_ms(200);
Attack5();
UP_delay_ms(200);
Attack6();
UP_delay_ms(200);
Sit();
UP_delay_ms(100);
}
else
{
Sit();
UP_delay_ms(10);
}
break;
}
}
}