广茂达机器人灭火程序_第1页
广茂达机器人灭火程序_第2页
广茂达机器人灭火程序_第3页
广茂达机器人灭火程序_第4页
广茂达机器人灭火程序_第5页
已阅读5页，还剩13页未读， 继续免费阅读

下载本文档

版权说明：本文档由用户提供并上传，收益归属内容提供方，若内容存在侵权，请进行举报或认领

文档简介

1、广茂达机器人灭火程序(纯C语言版)float tim_1=0.0; /*时间变量：不用*/float gf_1=0.0; /*速度功率值 */int mic_1=0; /*声音值,AI8或AI12*/int gi_1=0; /*用控制是否跳出某房间循环的量：任务完成与否控制量*/int gi_2=0; /*相对的左边火焰亮度值*/int gi_3=0; /*相对的左右边火焰亮度值的差值*/int gi_4=0; /*相对的右边火焰亮度值*/int gi_5=0; /*高中、小学程序切换*/int gi_6=0; /*二号房回家时用来数线的量*/int gi_7=0; /*第一次去二号房时看到的

2、有没有火的标记量*/int gi_8=0; /*地面灰度设置值*/int gi_9=0; /*侧面PSD测距值*/int gi_10=0; /*区分环境光与蜡烛光的参考值*/int ma_1=0; /*前左PSD值AI0*/int ma_2=0; /*正前PSD值AI1*/int ma_3=0; /*前右PSD值AI2*/int ma_4=0; /*后右PSD值AI3*/int ma_5=0; /*正后PSD值AI4*/int ma_6=0; /*后左PSD值AI5*/int ma_7=0; /*前地面灰度值AI6*/int ma_8=0; /*后地面灰度值AI7*/int ma_9=0; /

3、*回家时所用的地面灰度的二次测量值*/int ma_10=0; /*正前正后PSD测距值，例值 220*/int sci_1=0; /*指南针读值*/void main() SCI_Set(0,9600,0,8,1 );/*串口设置：指南针的端口设置，小学、初中不用*/ /*/ /*以下为调试参数*/ gi_10=450; /*区分环境光与蜡烛光的参考值，约比环境光最小值小50，例值：450*/ gi_8=310; /*地面灰度设置值，原则：稳定地区分黑白，取黑白值的中间值，例值：300*/ gi_9=280; /*侧面PSD测距值，例值： 280*/ ma_10=280; /*正前正后PSD

4、测距值，例值 280*/ gi_5=1;/*小学取1，初高中取0*/ gf_1=1.0;/*功率值，范围0.01.0*/ /*/ while(1) mic_1=AI(8); if(mic_1 750) break; /SetMotor(0x1111,100,100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0); wait( 0.300000 ); while(1) /*四号房*/ /*/ while(1) ma_7 = AI(6); if(ma_7 gi_8) gi_2=AI(9); gi_4=AI(11); if(gi_

5、2 gi_10) | (gi_4 gi_10) /SetMotor(0x1111,90,10,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(10*gf_1),0,0); /*有火时进四号房*/ wait( 0.100000 ); /SetMotor(0x1111,80,80,0,0); SetMotor(0x1111,(int)(80*gf_1),(int)(80*gf_1),0,0); /*有火时进四号房*/ wait( 0.050000 ); while(1) ma_7 = AI(6); gi_2=AI(9); if(ma_7 gi_8) & (gi

6、_2 300)/*四号灭火*/ StopMotor(0x1111); /SetMotor(0x1111,100,-100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0); wait( 0.080000 ); StopMotor(0x1111); DO( 0x1, 1 ); tim_1 =seconds(); wait( 0.500000 );/*机器人停顿灭火时间*/ DO( 0x1, 0 ); /*gi_5=1;*小学*/ break; else SubRoutine_2 (); if(gi_5 = 1) gi_1=1;

7、 break; SubRoutine_3 (); gi_1 = 1 ; break; else /*出四号房*/ StopMotor(0x1111); wait( 0.180000 ); /SetMotor(0x1111,-60,-100,0,0); SetMotor(0x1111,(int)(-40*gf_1),(int)(-100*gf_1),0,0); wait( 0.300000 ); break; else SubRoutine_1 (); if(gi_1 = 1) break; /*三号房*/ /*/ while(1) ma_8 = AI(7); if(ma_8 gi_8) gi_

8、2=AI(13); gi_4=AI(15); if(gi_2 gi_10) | (gi_4 gi_10) /SetMotor(0x1111,-80,-10,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-10*gf_1),0,0); /*有火时进三号房*/ wait( 0.100000 ); while(1) ma_8 = AI(7); gi_2=AI(15); if(ma_8 gi_8) & (gi_2 300) /*三号灭火*/ StopMotor(0x1111); DO( 0x10, 1 ); tim_1 =seconds(); wait( 0.

9、500000 );/*机器人停顿灭火时间*/ DO( 0x10, 0 ); /*gi_5=1;*小学*/ break; else SubRoutine_5 (); if(gi_5 = 1) gi_1=1; break; SubRoutine_6 (); gi_1 = 1 ; break; else StopMotor(0x1111); /*没火时出三号房*/ wait( 0.180000 ); /SetMotor(0x1111,100,15,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(15*gf_1),0,0); wait( 0.400000 );

10、break; else SubRoutine_4 (); if(gi_1 = 1) break; /*一（-2-1）号房*/ /*/ while(1) ma_7 = AI(6); if(ma_7 gi_8) gi_2=AI(9); gi_4=AI(11); if(gi_2 gi_10) | (gi_4 gi_10)/*此处不用改（第一次去二号门口测光，不进）*/ gi_7 = 1 ; StopMotor(0x1111); /*第一次从二号门出*/ wait( 0.180000 ); /SetMotor(0x1111,-80,-20,0,0); SetMotor(0x1111,(int)(-10

11、*gf_1),(int)(-80*gf_1),0,0); wait( 0.40000 ); while(1) ma_8 = AI(7); if(ma_8 gi_8) gi_2=AI(13); gi_4=AI(15); if(gi_2 gi_10) | (gi_4 gi_10) /SetMotor(0x1111,-80,-15,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-15*gf_1),0,0); /*有火时进一号房*/ wait( 0.200000 ); while(1) ma_8 = AI(7); gi_2=AI(15); if(ma_8 gi

12、_8) & (gi_2 300) /*一号灭火*/ StopMotor(0x1111); DO( 0x10, 1 ); tim_1 =seconds(); wait( 0.500000 ); /*机器人停顿灭火时间*/ DO( 0x10, 0 ); /*gi_5=1;*小学*/ break; else SubRoutine_5 (); if(gi_5 = 1) gi_1=1; break; SubRoutine_10 (); gi_1 = 1 ; break; else StopMotor(0x1111); wait( 0.180000 ); /SetMotor(0x1111,-60,60,0

13、,0); SetMotor(0x1111,(int)(-60*gf_1),(int)(60*gf_1),0,0); /*最后去2号房之前（即从一号去二号）*/ wait( 0.020000 ); StopMotor(0x1111); /SetMotor(0x1111,100,55,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(55*gf_1),0,0); ma_2 = AI(1); ma_3 = AI(2); while(ma_2 = ma_10) & (ma_3 = gi_9)/*最后去2号房之前*/ ma_2 = AI(1); ma_3 = AI(

14、2); /*最后去2号房之前*/ SetMotor(0x1111,(int)(70*gf_1),(int)(70*gf_1),0,0); wait( 0.2 ); SetMotor(0x1111,(int)(50*gf_1),(int)(-50*gf_1),0,0); wait( 0.2 ); gi_1=2; break; else SubRoutine_4 (); if(gi_1 = 2) | (gi_1 = 1) break; if(gi_1 = 2) | (gi_1 = 1) break; else SubRoutine_7 (); if(gi_1 = 1) break; /*二号房*/

15、 /*/ while(1) ma_7 = AI(6); if(ma_7 gi_8) gi_2=AI(9); gi_4=AI(11); if(gi_7=1)gi_2=10;/*第一次已*/ /*/ if(gi_2 1023) | (gi_4 gi_10) /SetMotor(0x1111,50,80,0,0); SetMotor(0x1111,(int)(50*gf_1),(int)(80*gf_1),0,0); wait( 0.100000 ); /SetMotor(0x1111,25,70,0,0); SetMotor(0x1111,(int)(10*gf_1),(int)(100*gf_1

16、),0,0); wait( 0.3500 ); while(1) ma_7 = AI(6); gi_2=AI(11); if(ma_7 gi_8) & (gi_2 gi_9) & (ma_2 ma_10) /SetMotor(0x1111,100,100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0); else if(ma_1 gi_9) & (ma_2 gi_9) & (ma_2 ma_10) /SetMotor(0x1111,-40,40,0,0); SetMotor(0x1111,(int)(-40*gf_1),(i

17、nt)(40*gf_1),0,0); else /SetMotor(0x1111,90,25,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(25*gf_1),0,0);/*行走过程中转弯速度*/ /wait( 0.001000 );void SubRoutine_2( ) /*趋光*/ gi_3=AI(9)-AI(11); if(gi_3 10) /SetMotor(0x1111,13,50,100,100); SetMotor(0x1111,(int)(13*gf_1),(int)(80*gf_1),100,100); else /SetMotor(0

18、x1111,90,90,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(90*gf_1),0,0); /wait( 0.001000 );/*/void SubRoutine_3( ) /*4号回家*/ /SetMotor(0x1111,-100,-40,100,100); SetMotor(0x1111,(int)(-100*gf_1),(int)(-40*gf_1),100,100); wait( 0.250000 ); DO( 0x111111, 0 ); ma_9 = 1023 ; resettime(); while(1) ma_8 = AI(7

19、); if(ma_8 gi_8) wait( 0.080000 ); ma_9 = AI(7); if(ma_9 = gi_8) StopMotor(0x1111); break; else tim_1 =seconds(); if(tim_1 gi_9) & (ma_5 ma_10) /SetMotor(0x1111,-100,-100,0,0); SetMotor(0x1111,(int)(-100*gf_1),(int)(-100*gf_1),0,0); else if(ma_6 gi_9) & (ma_5 gi_9) & (ma_5 ma_10) /SetMotor(0x1111,40

20、,-40,0,0); SetMotor(0x1111,(int)(40*gf_1),(int)(-40*gf_1),0,0); else /SetMotor(0x1111,-90,-25,0,0); SetMotor(0x1111,(int)(-90*gf_1),(int)(-20*gf_1),0,0);/*行走过程中转弯速度*/ /wait( 0.001000 );void SubRoutine_5( ) /*趋光B*/ gi_3=AI(13)-AI(15); if(gi_3 10) /SetMotor(0x1111,-60,-13,0,0); SetMotor(0x1111,(int)(-

21、80*gf_1),(int)(-13*gf_1),0,0); else /SetMotor(0x1111,-90,-90,0,0); SetMotor(0x1111,(int)(-90*gf_1),(int)(-90*gf_1),0,0); /wait( 0.001000 );/*/void SubRoutine_6( ) /*三号回家*/ /SetMotor(0x1111,-100,100,0,0); SetMotor(0x1111,(int)(-100*gf_1),(int)(100*gf_1),0,0); wait( 0.100000 ); StopMotor(0x1111); ma_7

22、 = AI(6); while(1) ma_7 = AI(6); if(ma_7 = 0.500000) break; else SubRoutine_1( ); StopMotor(0x1111); /SetMotor(0x1111,100,95,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(95*gf_1),0,0); ma_2 = AI(1); while(ma_2 500) ma_2 = AI(1); StopMotor(0x1111); /SetMotor(0x1111,-50,50,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(80*gf_1),0,0); wait( 0.200000 ); StopMotor(0x1111); ma_9 = 1023 ; while(1) ma_7 = AI(6); if(ma_7 gi_8) /SetMotor(0x1111,100,20,0,0); SetMotor(0x1111,(int)(100*gf_1),(int

人人文库> 全部分类> 教育资料 > 课件下载

温馨提示

  • 1. 本站所有资源如无特殊说明，都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等，如果需要附件，请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸，网页内容里面会有图纸预览，若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间，仅对用户上传内容的表现方式做保护处理，对用户上传分享的文档内容本身不做任何修改或编辑，并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容，请与我们联系，我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

最新文档

评论

0/150

提交评论