```c
#include "main.h"
//函数列表
#define g_I_F_Left_0 EXP_analog(0,KZ[1][7])
#define g_I_F_Left_1 EXP_analog(0,KZ[1][6])
#define g_I_F_Left_2 EXP_analog(0,KZ[1][5])
#define g_I_F_Left_3 EXP_analog(0,KZ[1][4])
#define g_I_F_Left_4 EXP_analog(0,KZ[1][3])
#define g_I_F_Left_5 EXP_analog(0,KZ[1][2])
#define g_I_F_Left_6 EXP_analog(0,KZ[1][1])
#define g_I_F_Left_7 EXP_analog(0,KZ[1][0])
#define g_I_B_Left_0 EXP_analog(1,KZ[1][7])
#define g_I_B_Left_1 EXP_analog(1,KZ[1][6])
#define g_I_B_Left_2 EXP_analog(1,KZ[1][5])
#define g_I_B_Left_3 EXP_analog(1,KZ[1][4])
#define g_I_B_Left_4 EXP_analog(1,KZ[1][3])
#define g_I_B_Left_5 EXP_analog(1,KZ[1][2])
#define g_I_B_Left_6 EXP_analog(1,KZ[1][1])
#define g_I_B_Left_7 EXP_analog(1,KZ[1][0])
#define F_M_R analog(1)
#define F_M_L analog(5)
#define a2 analog(2)
#define ri analog(11)
#define a7 analog(7)
#define g_I_AIL_M_Left analog(3)
#define g_I_AIL_M_Right analog(4)
#define d5 digital(5)
#define d6 digital(6)
#define d7 digital(7)
#define d8 digital(8)
#define d9 digital(9)
#define d10 digital(10)
#define d11 digital(11)
#define d12 digital(12)
#define IIC_analog(ch) iicdata(0xA8,ch)
#define IIC_digital(ch) iicdata(0xA8,32+ch)
#define IIC_servos(ch,s) iicdata(0xA8,64+ch);iicdata(0xA8,s)
#define IIC_servo_stop(ch) iicdata(0xA8,96+ch)
int i=0;
int z[5]={250,170,130,70,120};
int j=0;
int c;
int x;
int b;
int t;
int q;
int w;
int e;
int k1=2400;
int k2=3000;
int k3=200;
int t;
int m;
int n;
int k;
int r;
int w;
int s;
int f;
int i1;
int q1;
int w1;
int e1;
void run();
void go_and_up1();
void go_and_up2();
void go_and_up3();
void go_and_up4();
void go_and_up1_2();
void goout();//出框
void xunxian01();
void xunxian03();
void xunxian04();
void xunxian02();//巡线
void dingwei();
void turnleft(); //左转弯
void turnright();//右转弯
void b_turnright74();//后右转
void b_turnleft74();//后左转
void B_xunxian();//后巡线
void chafang();//查房
void kua();
void turnleft01();
void Link_back_speed20();
void Link_go_speed20();
void xiuxi();
void turnright01();
void Servos_SET(int SER_1,int SER_6,int SER_7,int SER_8,int SER_5);
//主函数开始
int main()
{
int t=21000;
int m=60000;
int n=80000;
int r;
int w;
int k;
int s;
int f;
int i;
int i1=0;
int q1=150;
int w1=130;
int e1=50;
RobotInit();
stop();sleep(0.1);
Servos_SET(250,170,130,70,120); //舵机的初始值
stop();sleep(1.0);
goout();//出门函数，用灰度传感器
while(t){
xunxian01();
t--;
}//一号线，只用巡线
turnleft();//左转，for转
xunxian03();//巡线，无红外
dingwei(); //定位，准备转弯
b_turnright74();//后右转，准备查房
chafang();//查房函数，使用红外测距传感器
while(m){
--m;
xunxian01();
}//查房完毕，回来，在3号线
turnright();//右转，for转
while(n){
--n;
xunxian04();
}//高速巡线
xunxian03();//低速巡线
dingwei();//定位，准备转弯
b_turnleft74();//后左转，准备查房
chafang();//查右病房，红外测距
xunxian03();//巡线，无红外
dingwei();//定位
turnleft01(); //在五号线末左转，定位转
xunxian03();//四号线，无红外巡线
dingwei();//定位转，路径太短
turnright01();//在四号线末，定位式右转
xunxian03();//六号线，无红外式巡线
dingwei();//定位转弯
turnleft01();//在六号线末，定位式左转
r=95000;
while(r){
xunxian01();
--r;
}//在七号线上，次数转弯
turnright();//在七号线末，for式右转
xunxian03();//无红外式巡线，八号线上
dingwei();//定位，准备转弯
turnleft01();//定位式左转，在八号线末
w=45000;
while(w){
Link_back_speed20();
w--;
}//后退，准备好手臂
go_and_up1();//拿药瓶的前进举爪go_and_up1();//拿药瓶的前进举爪//前进，去抓杯子
dingwei();
run(10,10);
sleep(0.2);
//////////////////////抓杯子//////////////////////////////////////////////
Servos_SET(250,170,130,70,120);stop();sleep(1);
Servos_SET(170,170,130,70,120);stop();sleep(1);
Servos_SET(170,200,130,70,120);stop();sleep(1);
z[0]=170;z[1]=200;z[2]=130;z[3]=70;z[4]=120;//抓杯子
/////////////////////////机械臂向上移动////////////////
go_and_up2();
q=160;
r=45000;
while(r){
Link_back_speed20();
r--;//后退，去放杯子
}
while(g_I_AIL_M_Left
run(-10,-10);
}//定位，准备右转
x=1;
while(1){
if(g_I_F_Left_0>k1)
break;
else
run(17,-17);
}
while(1){
if(g_I_AIL_M_Right>k2)
break;
else
run(17,-17);
if(g_I_AIL_M_Left>k2)
x=0;
}
if(x==1){
while(g_I_AIL_M_Left
run(17,-17);
}
} //转弯，准备去放杯子
//用灰度传感器转弯
while(1){
if(g_I_B_Left_7>k1&&g_I_B_Left_7>k1)
break;
else if(g_I_B_Left_7k1)
run(0,-20);
else if(g_I_B_Left_7>k1&&g_I_B_Left_0
run(-20,0);
else
run(10,10);
}//微调，成90度角转弯
while(1)
{
if(g_I_F_Left_0>k1)break;
else if(g_I_F_Left_7>k1)break;
else if(g_I_F_Left_3>k1)break;
else if(g_I_F_Left_4>k1)break;
else
run(15,15);//送杯子
go_and_up3();//放药瓶的前进举爪///
}
while(1)
{
if(g_I_F_Left_7>k1&&g_I_F_Left_0>k1)
break;
else if(g_I_F_Left_7k1)
run(-20,0);
else if(g_I_F_Left_7>k1&&g_I_F_Left_0
run(0,-20);
else
run(10,10);
}//微调，用 g_I_F_Left_7>k1&&g_I_F_Left_0>k1来微调，以此来调整中心
run(15,15);
sleep(0.2);
run(0,0);
sleep(0.1);
Servos_SET(170,125,60,210,120);stop();sleep(1);
Servos_SET(250,125,55,210,120);stop();sleep(0.5);
z[0]=250;z[1]=125;z[2]=55;z[3]=210;z[4]=120;//放杯子完毕
go_and_up4();//放药瓶的后退举爪，回到白线上
run(0,0);sleep(0.2);
while(g_I_F_Left_3
while(g_I_AIL_M_Left>k2){
run(17,-17);
}
while(g_I_AIL_M_Right>k2){
run(17,-17);
}
}
run(0,0);
sleep(0.2);
run(0,0);sleep(0.3);//在白线上微调，以为了后期巡线
f=0;
while(g_I_F_Left_1
xunxian01();
}//巡线
stop();
sleep(0.1);
run(10,8);
sleep(0.1);//左边在白线上
xunxian03();
while(1){
if(ri<200) break;
else{
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1){
run(15,15);
}
else
if(g_I_F_Left_2>k1){
run(10,30);
}
else
if(g_I_F_Left_5>k1){
run(30,10);
}
else
if(g_I_F_Left_1>k1){
run(-5,40);
}
else
if(g_I_F_Left_6>k1){
run(40,-5);
}
else run(15,15);
}
}//冲过任意十字路口
go_and_up1_2();//拿药瓶的前进举爪2，准备抓杯子
run(10,10);
sleep(0.3);
while(1){
if(g_I_AIL_M_Left>k2)
{
break;
}
else if(g_I_AIL_M_Right>k2)
{
break;
}
else run(10,10);
}
run(15,15);
sleep(0.25);
/////////////////////////抓右杯子////////////////////////////////////
printf("抓右杯子\n");
Servos_SET(250,170,130,70,120);stop();sleep(1);
Servos_SET(170,170,130,70,120);stop();sleep(1);
Servos_SET(170,200,130,70,120);stop();sleep(1);
z[0]=170;z[1]=200;z[2]=130;z[3]=70;z[4]=120;//抓右杯子
///////////////////////////////////////////////////////
///////////////////////task8//////////////////////////
q=160;
go_and_up2();//拿药瓶的后退举爪前进，抓杯子后抬高手臂
w=90000;
while(w){
Link_back_speed20();
w--;
}//已抓杯子，后退十万次
while(g_I_AIL_M_Left
run(-10,-10);
}//定位3十字路口，准备左转
for(w=0;w<660000;w++){
run(-17,17);
}//左转，准备送杯子
while(1){
if(g_I_B_Left_7>k1&&g_I_B_Left_7>k1){
break;
}
else
if(g_I_B_Left_7k1)
run(0,-20);
else
if(g_I_B_Left_7>k1&&g_I_B_Left_0
run(-20,0);
else
run(10,10);
}//转弯后微调
go_and_up3();//送杯子，放药瓶的前进举爪放药瓶的前进举爪
while(1)
{
if(g_I_F_Left_7>k1&&g_I_F_Left_0>k1)
break;
else if(g_I_F_Left_7k1)
run(-20,0);
else if(g_I_F_Left_7>k1&&g_I_F_Left_0
run(0,-20);
else
run(10,10);
}//微调，用 g_I_F_Left_7>k1&&g_I_F_Left_0>k1来微调，以此来调整中心
run(15,15);
sleep(0.2);
run(0,0);
sleep(0.1);
Servos_SET(170,125,60,210,120);stop();sleep(1);
Servos_SET(250,125,55,210,120);stop();sleep(0.5);
z[0]=250;z[1]=125;z[2]=55;z[3]=210;z[4]=120;
go_and_up4(); //回到白线上，放药瓶的后退举爪放药瓶的后退举爪
run(17,-17);
sleep(0.1);
run(15,-15);sleep(0.1);
run(25,-25);sleep(0.1);
while(g_I_F_Left_7
run(17,-17);
}
run(0,0);sleep(0.2);
while(g_I_F_Left_3
{
run(17,-17);
}
run(0,0);sleep(0.3);//回到白线上，微调后，巡线
f=0;
while(g_I_F_Left_1
xunxian01();
}
stop();
sleep(0.1);
while(g_I_F_Left_1>k1){//左边在白线上
run(10,10);
sleep(0.2);
continue;
}
while(g_I_F_Left_6>k1){
for(i=0;i<10000;i++){
if(g_I_F_Left_1>k1){
f=1;
}
run(15,15);
if(g_I_F_Left_1>k1){
f=1;
}
}
if(f==1){
run(10,10);
sleep(0.2);
continue;
}
}
run(10,8);
sleep(0.1);
while(1){
if(ri<200) break;
else{
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1)
run(15,15);
else if(g_I_F_Left_2>k1) run(10,30);
else if(g_I_F_Left_5>k1) run(30,10);
else if(g_I_F_Left_1>k1) run(-5,40);
else if(g_I_F_Left_6>k1) run(40,-5);
else run(15,15);
}
}//冲过任意十字路口
dingwei();
turnright01();
xunxian03();
dingwei();
turnright01();
r=95000;
while(r){
xunxian01();
--r;
}//在七号线上，次数转弯
turnleft();
w=90000;
while(w){
xunxian01();
--w;
}
while(1){
stop();
}
}
void go_and_up1()//拿药瓶的前进举爪1
{j=0;
while(1)
{
if(g_I_F_Left_7>k1) break;
else
{
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1) run(20,20);
else if(g_I_F_Left_2>k1) run(10,30);
else if(g_I_F_Left_5>k1) run(30,10);
else if(g_I_F_Left_1>k1) run(-5,30);
else if(g_I_F_Left_6>k1) run(30,-5);
else run(20,20);
if(j00==0)
{
if(z[1]<200) z[1]++;
Servos_SET(z[0],z[1],z[2],z[3],z[4]);
j=0;
}
j++;
}
}
}
void dingwei_back()//反方向巡各种路口
{
while(1)
{if(g_I_AIL_M_Left>b||g_I_AIL_M_Right>b) break;
else run(-15,-15);
}
run(0,0);
sleep(0.1);
}
void dingwei_backs()//确保能够让车回到白线上
{
while(1)
{
if(ri<200) break;
else run(-15,-15);
}
dingwei_back();
}
void go_and_up1_2(){//拿药瓶的前进举爪2
j=0;
while(1){
if(g_I_F_Left_0>k1) break;
else
{
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1)
run(20,20);
else
if(g_I_F_Left_2>k1)
run(10,30);
else
if(g_I_F_Left_5>k1)
run(30,10);
else
if(g_I_F_Left_1>k1)
run(-5,30);
else
if(g_I_F_Left_6>k1)
run(30,-5);
else
run(20,20);
if(j00==0){
if(z[1]<200) z[1]++;
Servos_SET(z[0],z[1],z[2],z[3],z[4]);
j=0;
}
j++;
}
}
}
void go_and_up2(){ //拿药瓶的后退举爪
j=0;
while(1){
if(g_I_B_Left_0>k1||g_I_B_Left_7>k1) break;
else{
if(g_I_B_Left_3>k1&&g_I_B_Left_4>k1) run(-18,-18);
else if(g_I_B_Left_2>k1) run(-10,-30);
else if(g_I_B_Left_5>k1) run(-30,-10);
else if(g_I_B_Left_1>k1) run(-5,-30);
else if(g_I_B_Left_6>k1) run(-30,-5);
else run(-20,-20);
if(jP0==0){
if(z[1]>170) z[1]--;
Servos_SET(z[0],z[1],z[2],z[3],z[4]);
j=0;
}
j++;
}
}
}
void go_and_up3(){//放药瓶的前进举爪
j=0;
while(g_I_F_Left_0
run(15,15);
if(jP00==0){
if(z[1]>125) z[1]-=5;
if(z[2]>60) z[2]-=5;
if(z[3]<210) z[3]+=10;
Servos_SET(z[0],z[1],z[2],z[3],z[4]);
j=0;
}
j++;
}
}
void go_and_up4(){//放药瓶的后退放爪
j=0;
while(g_I_AIL_M_Left
run(-15,-15);
if(j 00==0){
if(z[1]<170) z[1]+=1;
if(z[2]<130) z[2]+=5;
if(z[3]>70) z[3]-=5;
Servos_SET(z[0],z[1],z[2],z[3],z[4]);
j=0;
}
j++;
}
}
void Servos_SET(int SER_1,int SER_2,int SER_3,int SER_4,int SER_5){
init_servos(1);
IIC_servos(5,SER_5);
IIC_servos(4,SER_4);
IIC_servos(3,SER_3);
IIC_servos(2,SER_2);
IIC_servos(1,SER_1);
}
void Link_back_speed20(){
if(g_I_B_Left_3>k1&&g_I_B_Left_4>k1)
{
run(-25,-25);
}
else
if(g_I_B_Left_2>k1)
{
run(-40,-10);
}
else
if(g_I_B_Left_5>k1)
{
run(-10,-40);
}
else
if(g_I_B_Left_1>k1)
{
run(-50,5);
}
else
if(g_I_B_Left_6>k1)
{
run(5,-50);
}
else
{
run(-25,-25);
}
}
void turnleft01(){
run(-15,15);sleep(0.1);
run(-25,25);sleep(0.1);
while(g_I_F_Left_0
{
run(-17,17);
}
while(g_I_F_Left_4
{
run(-17,17);
}
}
void run(g_I_L,g_I_R){
motor(0,g_I_R);
motor(1,g_I_L);
}
void b_turnleft74(){
run(-15,15);sleep(0.1);
run(-25,25);sleep(0.1);
while(g_I_B_Left_7
run(-17,17);
}
while(g_I_B_Left_3
run(-17,17);
}
}
void turnright01(){
run(15,-15);sleep(0.1);
run(25,-25);sleep(0.1);
while(g_I_F_Left_7
run(17,-17);
}
while(g_I_F_Left_3
run(17,-17);
}
}
void goout(){
while(g_I_AIL_M_Left
run(25,25);
}
}
void turnright(){
for(j=0;j<667000;j++){
run(25,0);
}
}
void chafang(){
while(d7!=1)
{
B_xunxian();
}
while(d8!=1)
{
B_xunxian();
}
motor(20,20);
sleep(0.19);
}
void B_xunxian(){
if(g_I_B_Left_3>k1&&g_I_B_Left_4>k1)
{
run(-25,-25);
}
else if(g_I_B_Left_2>k1)
{
run(-40,-10);
}
else if(g_I_B_Left_5>k1)
{
run(-10,-40);
}
else if(g_I_B_Left_1>k1)
{
run(-50,5);
}
else if(g_I_B_Left_6>k1)
{
run(5,-50);
}
else
{
run(-25,-25);
}
}
void xunxian01(){
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1){
run(25,25);
}
else
if(g_I_F_Left_2>k1){
run(5,40);
}
else
if(g_I_F_Left_5>k1){
run(40,5);
}
else
if(g_I_F_Left_1>k1){
run(-5,60);
}
else
if(g_I_F_Left_6>k1){
run(60,-5);
}
else{
run(25,25);
}
}
void xunxian03(){
while(g_I_F_Left_0
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1){
run(25,25);
}
else
if(g_I_F_Left_2>k1){
run(10,50);
}
else
if(g_I_F_Left_5>k1){
run(50,10);
}
else
if(g_I_F_Left_1>k1){
run(-5,60);
}
else
if(g_I_F_Left_6>k1){
run(60,-5);
}
else{
run(25,25);
}
}
}
void dingwei(){
while(g_I_AIL_M_Left
run(10,10);
}
}
void turnleft(){
for(j=0;j<650000;j++){
run(2,25);
}
}
void b_turnright74(){
run(15,-15);sleep(0.1);
run(25,-25);sleep(0.1);
while(1)
{
if(g_I_B_Left_7>k1)
{
break;
}
else
{
run(17,-17);
}
}
while(1)
{
if(g_I_B_Left_3>k1)
{
break;
}
else
{
run(17,-17);
}
}
}
void xunxian04(){
if(g_I_F_Left_3>k1&&g_I_F_Left_4>k1){
run(35,35);
}
else
if(g_I_F_Left_2>k1){
run(10,50);
}
else
if(g_I_F_Left_5>k1){
run(50,10);
}
else
if(g_I_F_Left_1>k1){
run(-5,60);
}
else
if(g_I_F_Left_6>k1){
run(60,-5);
}
else{
run(35,35);
}
}
```
2014年08月03号机器人程序
最新推荐文章于 2026-01-05 10:27:08 发布
扫一扫
632
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
