搬运色块巡游景点巡线机器人小车的设计资料下载 -

/************************************************
程序功能：搬运1个色块: E点，黑色
舵机连接：左轮 P1_1
右轮 P1_0
QTI传感器连接：从左至右 P2_4 P2_3 P2_2 P2_1
程序编写调试：王淑鸿 2016-6
************************************************/
#include <Boebot.h>
#include <uart.h>
#define uint unsigned int
#define uchar unsigned char
void fastforward(void); //快速前进1步
void slowforward(void); //慢速前进1步
void slowerback(void); // 慢速后退1步
void turn_left(uint n); //左转
void turn_right(uint n); //右转
void zhixing(int n); //向前直行
void daotui(int i); //后退
void check_left(void); //小幅向左
void check_right(void); //小幅向右
void Get_QTI_State(void); //获取传感器返回值
void Follow_Line(void); //巡线
void Get_E(void) ; //取E点物块，取到中心线上
void Put_Black(void); // 将中心线上的黑色色块推到目标位置
void BackToZero(void); //从中心线回到起始区域
uchar QTIState;
uint RIGHT90=20;
uint RIGHT45=10;
uint RIGHT180=35;
uint LEFT90=25;
uint LEFT45=13;
void fastforward(void) //快速前进
{
P1_0=1;
delay_nus(1350);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1650);
P1_1=0;
delay_nms(20);
}
void slowforward(void) //慢速前进
{ P1_0=1;
delay_nus(1450);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1550);
P1_1=0;
delay_nms(20);
}
void slowerback(void) // 慢速后退
{
P1_0=1;
delay_nus(1550);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1450);
P1_1=0;
delay_nms(20);
}
void turn_left(uint n) //左转
{
int i;
for(i=0;i<=n;i++)
{
P1_0=1;
delay_nus(1450);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1450);
P1_1=0;
delay_nms(20);
}
}
void turn_right(uint n) //右转
{
int i;
for(i=0;i<=n;i++)
{
P1_0=1;
delay_nus(1550);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1550);
P1_1=0;
delay_nms(20);
}
}
void zhixing(int n) //直行
{
int a;
for(a=0;a<=n;a++)
fastforward();
}
void daotui(int n) //慢速倒退
{
int a;
for(a=0;a<=n;a++)
slowerback();
}
void check_left(void) //小幅向左
{
P1_0=1;
delay_nus(1400);
P1_0=0;
delay_nms(20);
P1_1=1;
delay_nus(1500);
P1_1=0;
delay_nms(20);
}
void check_right(void) //小幅向右
{
P1_1=1;
delay_nus(1600);
P1_1=0;
delay_nms(20);
P1_0=1;
delay_nus(1500);
P1_0=0;
delay_nms(20);
}
void Get_QTI_State(void) //获取传感器返回值
{
QTIState=P2&0x1e; //P2_4 P2_3 P2_2 P2_1
QTIState=QTIState/2; //右移1位
}
void Follow_Line(void) //巡线
{
Get_QTI_State();
switch(QTIState)
{
case 4: check_left() ;break; //传感器返回值：0100
case 2: check_right();break; //传感器返回值：0010
case 8: check_left() ;break; //传感器返回值：1000
case 12: check_left() ;break; //传感器返回值：1100
case 14: check_left() ;break; //传感器返回值：1110
case 1: check_right();break; //传感器返回值：0001
case 3: check_right();break; //传感器返回值：0011
case 7: check_right();break; //传感器返回值：0111
default: slowforward();break;
}
}
void Get_E(void) //取E点物块，取到中心线上
{
int i;
while(QTIState!=0x0f) //当不是全黑时，巡线
Follow_Line();
zhixing(6); //直行一小段，调整位置
turn_right(RIGHT90); //右转90度
while(QTIState!=0x00) //当不是全白时，巡线
Follow_Line();
zhixing(6);
turn_right(RIGHT180);
while(QTIState!=0x0f) //当不是全黑时，巡线
Follow_Line();
zhixing(6);
turn_left(LEFT90); //左转90度
for(i=0;i<40;i++)
Follow_Line();
turn_right(RIGHT180);
}
void Put_Black(void) // 将中心线上的黑色色块推到目标位置
{
int i;
while(QTIState!=0x0f) //当不是全黑时，巡线
Follow_Line();
zhixing(6);
turn_right(RIGHT45);
while(QTIState!=0x00) //当不是全白时，巡线
Follow_Line();
zhixing(6);
while(QTIState!=0x0f) //当不是全黑时，巡线
Follow_Line();
daotui(20);
turn_right(RIGHT180);
while(QTIState!=0x00) //当不是全白时，巡线
Follow_Line();
zhixing(6);
while(QTIState!=0x0f) //当不是全黑时，巡线
Follow_Line();
zhixing(6);
turn_left(LEFT45);
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码