|
|
void dao_w() //推到白，紅，黑區域的定位程序
{
while((!P14_state())||(!P16_state()))
{
//中線控制
//大右偏判斷
if((P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1500, 1350,1); //左轉修正
}
//機器人小右偏判斷 x 1 0 x
else if((P22_state()&&(!P23_state()))||(P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1550, 1400,1); //左轉修正
}
//大左偏判斷
else if(((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1650, 1500,1); //右轉修正
}
//機器人小左偏判斷 x 0 1 x 或 0 0 0 1
else if(((!P22_state())&&P23_state())||((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1600, 1450,1); //右轉修正
}
else if(P21_state()&&P22_state()&&P23_state()&&P24_state()) //到十字路口, 1 1 1 1
{
break; //退出循環，下一個任務
}
else //其他情況
{
motor_motion(LEFT_QX, 1400, 1); //前進 1584, 1420, 1 1683, 1320, 1
}
}
}
void dao_lan() // 推到藍色的定位
{
while(!P14_state())
{
//中線控制
//大右偏判斷
if((P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1500, 1350,1); //左轉修正
}
//機器人小右偏判斷 x 1 0 x
else if((P22_state()&&(!P23_state()))||(P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1550, 1400,1); //左轉修正
}
//大左偏判斷
else if(((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1650, 1500,1); //右轉修正
}
//機器人小左偏判斷 x 0 1 x 或 0 0 0 1
else if(((!P22_state())&&P23_state())||((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1600, 1450,1); //右轉修正
}
else if(P21_state()&&P22_state()&&P23_state()&&P24_state()) //到十字路口, 1 1 1 1
{
break; //退出循環，下一個任務
}
else //其他情況
{
motor_motion(LEFT_QX, 1400, 1); //前進 1584, 1420, 1 1683, 1320, 1
}
}
}
void dao_huang() //推到黃色區域的定位
{
while(!P16_state())
{
//中線控制
//大右偏判斷
if((P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1500, 1350,1); //左轉修正
}
//機器人小右偏判斷 x 1 0 x
else if((P22_state()&&(!P23_state()))||(P21_state()&&(!P22_state())&&(!P23_state())&&(!P24_state())))
{
motor_motion(1550, 1400,1); //左轉修正
}
//大左偏判斷
else if(((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1650, 1500,1); //右轉修正
}
//機器人小左偏判斷 x 0 1 x 或 0 0 0 1
else if(((!P22_state())&&P23_state())||((!P21_state())&&(!P22_state())&&(!P23_state())&&P24_state()))
{
motor_motion(1600, 1450,1); //右轉修正
}
else if(P21_state()&&P22_state()&&P23_state()&&P24_state()) //到十字路口, 1 1 1 1
{
break; //退出循環，下一個任務
}
else //其他情況
{
motor_motion(LEFT_QX, 1415, 1); //前進 1584, 1420, 1 1683, 1320, 1
}
}
}
//QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ////////////////
//從起點到A,B,C,D,E
void Q_A()
{
to_shizilukou1();
motor_motion(1620,1400,20);
to_shizilukou2();
motor_motion(LEFT_QX,1400,6) ; //第二十字路樓口到A點前
fan90_L();
delay_nms(10);
to_zou(70);
delay_nms(100);
//motor_motion(LEFT_QX,1400,20); //前行
}
void Q_B()
{
to_shizilukou1();
motor_motion(1620,1400,20);
to_shizilukou2();
motor_motion(LEFT_QX,1400,6); //第二十字路樓口到B點前
fan45_L();
delay_nms(10);
to_zou(70);
delay_nms(100);
motor_motion(LEFT_QX,1400,30); //前行
}
void Q_C()
{
to_shizilukou1();
motor_motion(LEFT_QX,1400,20);
to_shizilukou2();
to_zou(70); //第二十字路樓口到C點前
delay_nms(100);
}
void Q_D()
{
to_shizilukou1();
motor_motion(1620,1400,20);
to_shizilukou2();
motor_motion(LEFT_QX,1400,7) ; //第二十字路樓口到D點前
fan45_R();
delay_nms(10);
to_zou(70);
motor_motion(LEFT_QX,1400,30); //前行
}
void Q_E()
{
to_shizilukou1();
motor_motion(1620,1400,20);
to_shizilukou2();
motor_motion(LEFT_QX,1400,7) ; //第二十字路樓口到E點前
fan90_R();
delay_nms(10);
to_zou(70);
motor_motion(LEFT_QX,1400,30); //前行
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// 將黃色物塊推到黃色靶心后到B,C,D,E,
void Huang_to_B()
{
daotui();
fan180();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan135_L();
delay_nms(10);
to_zou(70);
}
void Huang_to_C()//90ok
{
daotui();
fan180();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
fan90_L();
delay_nms(10);
to_zou(70); //
}
void Huang_to_D()//45ok
{
daotui();
fan180();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan45_L();
delay_nms(10);
to_zou(70);
}
void Huang_to_E()//180ok
{
daotui();
fan180();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,33);
to_zou(70);
}
//從黃色靶心回到第二 十字路口車頭正對起點
void Huang_to_shizilukou()
{
daotui();
fan180();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
fan90_L();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,14);
motor_motion(1600, 1600,1); //往右偏點
}
//將A點的物快推到各靶心
void A_huang()
{
dao_huang();
}
void A_bai()
{
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan135_L();
delay_nms(10);
to_zou(70);
dao_w();
}
void A_hong()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
fan90_L();
delay_nms(10);
to_zou(70);
dao_w(); //
}
void A_hei()//45ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan45_L();
delay_nms(10);
to_zou(60);
dao_w();
}
void A_lan()//180ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,30);
to_zou(60);
dao_lan();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//將物塊推到白色區域后，到A,C,D,E點
void Bai_to_A()//135ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,9);
fan135_R();
delay_nms(10);
to_zou(60);
}
void Bai_to_C()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,15);
fan135_L();
delay_nms(10);
to_zou(60);
}
void Bai_to_D()//90ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan90_L();
delay_nms(10);
to_zou(60);
}
void Bai_to_E()//45ok
{
daotui();;
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,20);
fan45_L();
delay_nms(10);
to_zou(60);
}
//BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB///////////////
void B_huang()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan135_R();
delay_nms(10);
to_zou(60);
dao_huang();
}
void B_bai()
{
dao_w();
}
void B_hong()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,15);
fan135_L();
delay_nms(10);
to_zou(60);
dao_w();
}
void B_hei()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan90_L();
delay_nms(10);
to_zou(60);
dao_w();
}
void B_lan()//45ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,20);
fan45_L();
delay_nms(10);
to_zou(60);
dao_lan();
}
//從B的白色區域回到第二十字路口車頭正對起點
void Bai_to_shizilukou()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,15);
fan135_L();
delay_nms(10);
to_zou(60);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,14);
motor_motion(1600, 1600,1); //往右偏點
}
//**************+++++++++++++++++++++++++++++++++=************************88
//從紅色區域去到每條路上的點
void Hong_to_A()//90ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan90_R();
delay_nms(10);
to_zou(60);
}
void Hong_to_B()//135ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,5);
fan135_R();
delay_nms(10);
to_zou(60);
}
void Hong_to_D()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan135_L();
delay_nms(10);
to_zou(60);
}
void Hong_to_E()//90ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan90_L();
delay_nms(10);
to_zou(60);
}
//CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC//////////////
void C_huang()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan90_R();
delay_nms(10);
to_zou(60);
dao_huang();
}
void C_bai()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,5);
fan135_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void C_hong()
{
dao_w();
}
void C_hei()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,13);
fan135_L();
delay_nms(10);
to_zou(60);
dao_w();
}
void C_lan()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan90_L();
delay_nms(10);
to_zou(60);
dao_lan();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//從C的紅色區域回到第二十字路口車頭正對起點
void Hong_to_shizilukou()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,14);
motor_motion(1600, 1600,1); //往右偏點
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//將黑色靶心到A,B,C,E點
void Hei_to_A()//45ok
{
daotui;
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan45_R();
delay_nms(10);
to_zou(60);
}
void Hei_to_B()//90ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan90_R();
delay_nms(10);
to_zou(60);
}
void Hei_to_C()//135ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan135_R();
delay_nms(10);
to_zou(60);
}
void Hei_to_E()//135ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,15);
fan135_L();
delay_nms(10);
to_zou(60);
}
//DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD//////////
void D_huang()//45ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan45_R();
delay_nms(10);
to_zou(60);
dao_huang();
}
void D_bai()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan90_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void D_hong()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan135_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void D_hei()
{
dao_w();
}
void D_lan()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan135_L();
delay_nms(10);
to_zou(60);
dao_lan();
motor_motion(LEFT_QX, 1400,2);
}
//從D的黑色區域回到第二十字路口車頭正對起點
void Hei_to_shizilukou()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan135_R();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,14);
motor_motion(1600, 1600,1); //往右偏點
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//從藍色靶心到A,B,C,D點
void Lan_to_A()//180ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
to_zou(60);
}
void Lan_to_B()//45ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan45_R();
delay_nms(10);
to_zou(60);
}
void Lan_to_C()//90ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,6);
fan90_R();
delay_nms(10);
to_zou(60);
}
void Lan_to_D()//135ok
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan135_R();
delay_nms(10);
to_zou(60);
dao_w();
}
//EEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE///////////////
void E_huang()//180ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
to_zou(60);
dao_huang();
}
void E_bai()//45ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,11);
fan45_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void E_hong()//90ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,6);
fan90_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void E_hei()//135ok
{
//to_zou(50);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan135_R();
delay_nms(10);
to_zou(60);
dao_w();
}
void E_lan()
{
dao_lan();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//從E的藍色區域回到第二十字路口車頭正對起點
void Lan_to_shizilukou()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,6);
fan90_R();
delay_nms(10);
to_zou(70);
motor_motion(1400, 1250,52); // fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,14);
motor_motion(1600, 1600,1); //往右偏點
}
//^%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*************************%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%************************
//借道代碼//借道代碼//借道代碼//借道代碼//借道代碼////
////然后將小車走回第二十字鹿樓口///////////////
void A_hui()
{ //to_zou(45);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
fan90_R();
delay_nms(10);
to_zou(70);
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
}
void B_hui()
{ //to_zou(45);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,8);
fan45_R();
delay_nms(10);
to_zou(70);
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
}
void C_hui()
{
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
to_zou(70);
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
}
void D_hui()
{ //to_zou(45);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan45_L();
delay_nms(10);
to_zou(70);
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
}
void E_hui()
{ //to_zou(45);
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,6);
fan90_L();
delay_nms(10);
to_zou(70);
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
}
//%%%%%%%%%%%%^****************************************%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//*****從各靶心到借道的位置拿回物快
void Huang_hui()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,12);
fan90_R();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
}
void Bai_hui()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,8);
fan45_R();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
}
void Hong_hui()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
}
void Hei_hui()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,10);
fan45_L();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
}
void Lan_hui()
{
daotui();
fan180();
delay_nms(10);
to_shizilukou2();
motor_motion(LEFT_QX, 1400,6);
fan90_L();
delay_nms(10);
to_zou(70);
fan180();
delay_nms(10);
to_shizilukou2();
}
//*******************************************************************************%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%**************
//從十字路去推F,G,H,I，推物快回到第二個十字路口
void shizilukou_to_F()
{
fan60_L();
motor_motion(LEFT_QX, 1400,80);
fan180(); //掉頭
delay_nms(10);
fan30_L();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,7);
fan90_R();
to_shizilukou2();
}
void shizilukou_to_G()
{
fan30_L();
motor_motion(LEFT_QX, 1400,80);
fan180(); //掉頭
fan60_L();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,5);
fan90_R();
to_shizilukou2();
}
void shizilukou_to_H()
{
fan30_R();
motor_motion(LEFT_QX, 1400,80);
fan180(); //掉頭
fan30_R();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,5);
fan90_L();
to_shizilukou2();
}
void shizilukou_to_I()
{
fan60_R();
motor_motion(LEFT_QX, 1400,70);
fan180(); //掉頭
fan60_R();
to_shizilukou2();
motor_motion(LEFT_QX, 1400,7);
fan90_L();
to_shizilukou2();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*************************
//十字路口推到靶心
void shizilukou_huang()
{
motor_motion(LEFT_QX,1400,10);
fan90_L();
to_zou(60);
dao_lan();
}
void shizilukou_bai()
{
motor_motion(LEFT_QX,1400,10);
fan45_L();
to_zou(60);
dao_w();
}
void shizilukou_hong()
{
motor_motion(LEFT_QX,1400,10);
to_zou(60);
dao_w();
}
void shizilukou_hei()
{
motor_motion(LEFT_QX,1400,10);
fan45_R();
to_zou(60);
dao_w();
}
void shizilukou_lan()
{
motor_motion(LEFT_QX,1400,10);
fan90_R();
to_zou(60);
dao_lan();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//****&*&*&*&*^&*%*^$十字路口去A,B,C,D,E點
void shizilukou_A()
{
motor_motion(LEFT_QX,1400,10);
fan90_L();
to_zou(60);
}
void shizilukou_B()
{
motor_motion(LEFT_QX,1400,10);
fan45_L();
to_zou(60);
}
void shizilukou_C()
{
motor_motion(LEFT_QX,1400,10);
to_zou(60);
}
void shizilukou_D()
{
motor_motion(LEFT_QX,1400,10);
fan45_R();
to_zou(60);
}
void shizilukou_E()
{
motor_motion(LEFT_QX,1400,10);
fan90_R();
to_zou(60); |
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
