“……我们对自动机的恐惧再次被《诗篇》所驾驭，因为空空的轮椅在一段悲伤而孤独的人生旅程中，沿着它的各种轨迹行进，一段无处可去的旅程。它的动作重复着，就像一个不断重复的记忆，一个对另一个生命和另一个旅程的记忆，就像唐纳德·罗德尼的父亲一样……” （展览手册，简·比尔顿。）
2016年11月5日，《重塑唐纳德·罗德尼》（Reimaging Donald Rodney），《生动项目》（Vivid Projects），伯明翰8号教堂 http://www.vividprojects.org.uk/programme/reimaging-donald-rodney/ 超人。 2012年7月19日至10月16日在威康信托基金会举办的展览 https://wellcomecollection.org/whats-on/exhibitions/superhuman网站 尼特·诺马尔 ：Beurs van Berlage，阿姆斯特丹，2009年12月16日至2010年3月8日： [su_youtube url=“ https://www.youtube.com/watch？v=Fe029H2Y8tc “width=”1600〃height=“1600”] 唐纳德·罗德尼回顾。 国际视觉艺术学院（INIVA），伦敦，2008年10月30日至11月29日 http://www.iniva.org/exphibitions_projects/2008/in_retrospect
“不应低估罗德尼作品的政治意义，也不应低估他继续激励年轻艺术家的遗产。” （Keith Piper，Co-curator）。
/*嘘！！ 1*///程序WCSHOW8.CTX //版本97年9月14日，98年3月15日，2003年2月，2008年10月（INIVA画廊伦敦） //版本2009年11月 //2012年6月-7月版威康展览 //轮椅项目97 //wcshow2.ctx的修正 //wcshow3.ctx中的轨迹编码 //来自wcshow6.ctx /*SH1！ 1*///变量//新建。 包括房间尺寸 /*HS！！ 2*//----------通信变量-------------- int Eureka=104，scope_data=110，gyro_data=105，gyror_reset=125 int reset_shaft_xyphi=120 int battery_data=106 整数i int ok_byte=33，ready_byte=44，sending_byte=55，command_send=66 int stop_byte=77，stop_motors=99，motor_command=107 int消息字节，b1，b2，b3 int serial_count，serial_cont_limit=500 //限制越小越好 浮点float_data int left_speed=1，right_speed=1 向右浮动速度所需=0 浮点left_speed_desired=0 /*HH2！ 2*//----------位置变量-------------- 浮子PI=4*弧（1） 浮点phi2，angle1，angle2，delta_phi，angle_difference 浮点x_wc=0，y_wc=0.，phi_wc=0 浮点x_wc_shift=0，y_wc_shift=零，phi_wc_shift=0 浮点old_x_wc=0，old_y_wc=0.，old_phi_wc=0 浮点dxwcdt 浮点x_wc_predict、y_wc_predict和phi_wc.predict 浮动上一个hi_wc、上一个x_wc和上一个y_wc 浮子x轴、y轴、phi轴 浮动w_x_shaft，w_y_shaft，w_phi_shaft，w0_shaft 浮子dx轴dt、dy轴dt和dphi轴dt 浮动旧x轴、旧y轴、旧ph轴 浮子陀螺、w0陀螺、wp陀螺、dphigyro_dt 浮点phi_camera、w_phi_camera和d_phi_camera_dt 浮点phi_cam1，phi_cam2 浮动x声纳、y声纳、phi声纳 浮动w0_sonar、wx_sonar，wy_sonar和w_phi_sonar 浮点dx_sonar_dt、dy_sonar-dt、dphi_sonar/dt 浮子last_time_shaft，last_time_motor /*HH2！ 2*//--------轮椅--------- 浮动增量r=56.5 /*HH2！ 2*//-------------威康信托的运营规模------------ 浮点max_room_xcm=1000//694 浮点数max_room_ycm=1100//1005 /*HH2！ 2*//----------------轨迹---------------//新 int节列表[32] /*HH2！ 2*//--------------神经网络--------------//新 浮点θ，θ_下一个，r int步数_nb_max=16 int k，当前网格 int节_大小[40] /浮子截面_boost=150 /基于旧节 /浮子截面_boost=50 /基于新节点的轨迹 float section_boost=90//使用1进行测试。 应该是90 /基于新节点的现场绘图 /浮子截面_boost=0.3 /基于非常新的节点+sigma的轨迹 浮点强制下一状态 /*HH2！ 2*//----------速度变量----------//新 浮子部分_速度[200] int current_section、temp_left_speed和temp_right_speed /*HH2！ 2*//------------声纳-------------- int键盘，颜色，声纳，alarm_persistence=0 浮点time_since_last_alarm=0，time_of_last_larm=0 int min_range_check=20，max_sonar_array=1500 int echo[max_sonar_array] 浮点查找标记，xs，ys 浮点sonar_dir[8+1]，sonar_range[8+1]，old_sonar_range[8+1 int前障碍，左障碍，右障碍 sonar_dir[1]=-PI/2 sonar_dir[2]=3*PI/4 sonar_dir[3]=-PI sonar_dir[4]=PI/2 sonar_dir[5]=PI/2 sonar_dir[6]=PI/2 sonar_dir[7]=0 sonar_dir[8]=PI/4 声纳_x[1]=0 声纳y[1]=-10 声纳_x[2]=-26 声纳y[2]=7 声纳_x[3]=-26 声纳y[3]=37 声纳_x[4]=-26 声纳_y[4]=51 声纳_x[5]=0 声纳_y[5]=51 声纳_x[6]=26 声纳_y[6]=51 声纳_x[7]=26 声纳_y[7]=37 声纳_x[8]=26 声纳y[8]=7 浮点sonar_d1、sonar_d2、sonar-d3、sonar/d4 /*HH2！ 2*///-----pcmcia卡-------- //int端口=256，端口b=257 int端口a=8192，端口b=8193 //int端口a=992，端口b=993 //int端口a=640，端口b=641 /（十六进制100，如autoexec.bat中dc24cl.com调用中所定义） /（十六进制3e0=992 dec，如autoexec.bat中dc24cl.com调用中所定义） //十六进制280=十进制640 /*HH2！ 2*////---------视频------------ 浮动对比度增益=5 浮点平均act=0，最大act=0、w0_camera 浮子xsize=0.97 浮子尺寸=0.97 浮动xcenter=0.5 浮动y中心=0.5 int节点id，max_search=50 int Houghinit=0 /*HH2！ 1*//-设置串口模式---- //ctx系统“模式com1:1200，n，8,1” //ctx系统“模式com2:9600，n，8,1” //int端口=2 //ctx系统“模式com1:9600，n，8,1” ctx系统“模式com1:9600，n，8,1，p” int端口=1 //巴基斯坦 /*HH1！ 1*///===定义按钮========= defpan程序 按钮“返回”，“弹出” 但差距 //按钮“发送”、“gosub发送” 按钮“STOP”，“gosub STOP_motors” 按钮“Motors_on”，“gosub send_motor_command” 但差距 //按钮“Scope”，“gosub get_Scope_data” 按钮“声纳”，“gosub Sonar_scan8” /但差距 /按钮“视频”，“gosub get_camer_data” /按钮“QC init”，“gosub Houghinitqc” 但差距 按钮“Reset_Gyro”，“gosub Reset_Gyro” 按钮“Gyro”，“gosub get_Gyro_data” 但差距 按钮“重置轴”、“gosub Reset_Shaft_position_to_zero” 按钮“Shaft_pos”，“gosub get_Shaft_position_data” 但差距 按钮“轨迹位置”，“gosub procedure_Track_position” pushpan程序 /*HH1！ 1*///图形和窗口 浮点最大步长=255 graph_default_gridtype 0 graph_default_axislabel“步骤”，“数据” graph_default_range 0，0，最大步长，255 makegraph数据 graph_default_axislabel“范围”，“传感器” graph_default_range 0，0，浮点（max_sonar_array），10 标记声纳 /win_default_range 0、0、1、1 /makewin图像 win_default_range-50，-50,50+最大房间_xcm，50+最大空间_ycm makewin房间 //win_plot 0，0，红色 //win_line 1000、1100、黄色 /*HH121*///构建网络并定义要使用的轨迹段 /gosub构建 /仅当使用图像时才需要 /gosub build_圆形 /*HS！！ 2*///定义节列表//新建-需要反映在“构建轨迹”中 i=1至31 节列表[i]=-99 下一个i //实际使用的部分： 节列表[1]=1 section_list[2]=2 节列表[3]=3 节列表[4]=4 节列表[5]=13 节列表[6]=14 截面列表[7]=15 节列表[8]=16 //现在需要按升序排列。 gosub构建的轨迹 /*HH1！ 1*/int ON_BRICKS=0//如果在BRICKS=1->sonar-based obst上。 避免禁用 int模拟=0 //--->如果上述值发生更改，则清除并运行程序 /*SH1！ 1*/结束 /*SS！！ 1*/ /*嘘！！ 1*/procedure_track_position://此处设置的初始位置 /*HS！！ 2*/重启程序： w0_轴=1 w0_陀螺=1 w0_camera=0 w0_sonar=1 报警持续时间=0 float l_arrow=最大room_xcm/15 /----------------------------------- /*HH2！ 2*///初始化机器人位置： /----------------------------------- //在这里，我们决定他从哪里开始，在哪里看。 //在最后的节目中，他会自己发现。 /gosub程序self_localization //x_wc=最大房间xcm/2 x_wc=700//对于Wellcome信托 y_wc=0 phi_wc=PI/2//朝向房间中部=北部 //phi_wc=0//向右。 //phi_wc=PI/2-0.35//北，略偏东。 /启动序列： //set_grid_outputs Sequence_State，1*section_boost set_grid_outputs Sequence_State，5*section_boost（设置网格输出序列状态） old_x_wc=x_wc 旧_y_wc=y_wc old_phi_wc=phi_wc 旧x轴=x wc old_y_shaft=y_wc 旧的phi_shaft=phi_wc gosub重置轴位置 gosub复位陀螺仪 旧x轴=x wc old_y_shaft=y_wc old_phi_shaft=phi_wc 如果模拟=1 浮动next_x=x_wc float next_y=y_wc float next_phi=phi_wc 结尾 /----------------------------------- /*HH2！ 2*///初始化机器人的速度和每个部分的速度//新 /----------------------------------- 浮点base_speed=20//15//13//最大值是多少？ //Wellcome适合20岁 left_speed=1 right_speed=1 gosub发送电机命令 //预加载电容器并防止 //倒退的开始 /测试 /基本速度=10 left_speed_desired=0 right_speed_desired=0 /---------------------------------------- //开始大圆圈------------- 对于k=0到12 截面速度[k]=0.8*基本速度//为0.7 下一个k k=13至24 截面速度[k]=0.8*基本速度 下一个k 对于k=25至48 截面速度[k]=基本速度 下一个k //开始向内螺旋------------ 对于k=49到60 截面速度[k]=0.9*基本速度 下一个k k=61至70 截面速度[k]=0.8*基本速度 下一个k k=71至78 分段速度[k]=0.6*基本速度 下一个k k=79至83 截面速度[k]=0.5*基本速度 下一个k k=84至87 截面速度[k]=0.4*基本速度 下一个k //八的起始数字------------ k=88至94 截面速度[k]=0.4*基本速度 下一个k k=95至156 截面速度[k]=0.5*基本速度 下一个k k=157至162 截面速度[k]=0.4*基本速度 下一个k //开始向外螺旋------------- k=163至166 分段速度[k]=0.4*基本速度 下一个k k=167至175 截面速度[k]=0.5*基本速度 下一个k k=176至185 截面速度[k]=0.7*基本速度 下一个k k=186至199 截面速度[k]=0.8*基本速度 下一个k /--------------------------------------------------------------- /*HH2！ 2*///主控制回路（获取位置数据、绘图、计算控制） /--------------------------------------------------------------- win_active聊天室 win_clear（清除） 打印“按任意键停止（但不按ESC）” /以便不在通信过程中停止 win_移动0,0 win_line max_room_xcm，0，4 win_line max_room_xcm，最大room_ycm，4 win_line 0，最大room_ycm，4 win_line 0、0、4 初始化计时器 last_time_shaft=0 last_time_motor=0 /*HH2！ 2*/同时！ kbhit（） 键盘=端口_in（96） //我的声音5000,30 如果经过时间（1）>100000 初始化计时器 last_time_shaft=0 last_time_motor=0 结尾 /*HH2！ 2*///gosub获取_轴_位置_数据 如果模拟=1 x轴=x_wc+0.3*（下一个x轴-x_wc） y轴=y_wc+0.3*（下一个y轴） phi_shaft=next_phi//+0.1*（next_pi-phi_wc） 其他的 gosub获取轴位置数据 结尾 w_x轴=0.5 w_y轴=0.5 w_phi_shaft=0.25 如果y轴=0&x轴=0 转到重新启动_程序 结尾 /win_line x_shaft、y_shaft，白色 /*HH2！ 2*/gosub预测电流轴位置 /gosub获取陀螺数据 /w_phi_gyro=0.25 /打印“Phi G Y R O=”，Phi_gyro*180/PI /*HH2！ 2*///gosub get_sonar_data 如果模拟= 1 gosub获取sonar_data 结尾 /*HH2！ 2*/gosub check_sonar_validity phi_sonar=phi_sonar+0.45*d _ phi_shaft _ dt /0.45将该值设置为RugWarrior /更新（相同的0.45用于将 /“check_sonar_validity”中的角度） /*HH2！ 2*///如有必要（机器人卡在角落里），重新校准 //如果alarm_persistence>6 //戈苏布重定标 //结尾 /gosub获取相机数据 /w_phi_camera=0 x_wc=（w_x_shaft*x _shaft+w_x_sonar*x _sonar）/（w_x _shave+w_x sonar） y_wc=（w_y_shaft*y_shaft+w_y_sonar*y_sonar）/（w_y_shaft+w_y_sonar） //打印“====++++++y_wc=”，y_wc，“y_shaft=”，y轴 /*SH2！ 2*///打印“====++++++++x_wc=”，x_wc，“x_shaft=”，x轴 /*HS！！ 2*///计算重量平均位置值 角度1=phi_shaft 角度2=phi_sonar 戈苏布角_diff /如果abs（angle_difference）>0.3*PI /w_phi_sonar=0 /结束语 phi_wc=（phi_shaft*（w_phi_shaft+w_phi_sonar）+w_phi_sonar*角度差异）/（w_phi_shaft+w _phi_sonar） /----wc位置在当前时间估计--- /稍后这将是加权平均数 /还包括延迟和变化率dbladt /*HH2！ 2*///重置RugWarrior板中的值 如果w_phi_sonar>0 | w_x_sonar>0 | w_y_sonar>0 角度1=0 角度2=phi_wc 戈苏布角_diff phi_wc=角度差异 gosub重置轴位置 /gosub复位陀螺仪 //我的声音1000,30 结尾 /*HH2！ 2*//----------位置和声纳图----------- win_fillellipse x_wc-0.1*l_arrow、y_wc-0.1%*l_arlow、x_wc+0.1*l_artow、LBLUE win_移动x_wc，y_wc win_line x_wc+l_arrow*cos（phi_wc），y_wc+l_arrow*sin（phi_wc），蓝色 //----------声纳图--------------- gosub声纳槽 /------------------------------ /*SH2！ 2*///使用神经网络计算新的电机指令 /------------------------------ /（此处简化） //left_speed=15 //right_speed=15 /gosub反馈_straight_line gosub预测位置控制操作 /gosub更新网络 /对于圆圈 gosub更新网络2 gosub计算新电机命令 /*HS！！ 2*///处理障碍物（停止或避开）或执行电机命令 temp_left_speed=左速 temp_right_speed=右侧速度 gosub sonar_saffety//砖块试验禁用 /*HH232*///->回避操作（如果是在砖上则不是） 如果ON_BRICKS=1 转到no_avoidance 结尾 如果报警计数器<=2//>=2 int motion_duration=4500//3200//2800//2400用于短距离回避动作 /*SS！！ 3*/ // 1. 前方障碍物情况->前向转弯 如果front_obstacle=1&left_obstale=0&right_obstable=0 如果delta_phi<0 left_speed=18//10 right_speed=1 其他的 left_speed=1 right_speed=18//10 结尾 gosub发送电机命令 ms_播放动作_持续时间 打印“逃生后停止” left_speed=0 right_speed=0 gosub发送电机命令 结束语 /*HS！！ 3*/ // 2. 左侧或右侧障碍物->转向逃生//新23.2.03 如果left_obstacle=1&right_obstale=0 left_speed=18//10 right_speed=1 gosub发送电机命令 ms_delay动作持续时间//1500 打印“逃生后停止” left_speed=0 right_speed=0 gosub发送电机命令 结束语 如果right_obstacle=1&left_obstale=0 left_speed=1 right_speed=18//10 gosub发送电机命令 ms_delay动作持续时间//1500 打印“逃生后停止” 左速度=0 right_speed=0 gosub发送电机命令 结尾 /*SH3！ 3*/ // 3. 两侧障碍物->不移动 如果right_obstacle=1&left_obstale=1 left_speed=1 right_speed=1 gosub发送电机命令 打印“两侧挤压” my_sound 3000，150//短促的高痒嘟嘟声 消息_播放1500 结尾 /*HS！！ 3*/ // 4. 如果前面太近->停止并发出哔哔声（注释掉） /如果声波范围[4]<0&声波范围[5]<0&声纳范围[6]<0 /我的声音6000100 /结束语 /*HH3！ 3*/ // 4. 逃生后停车 //打印“逃生后停止”-如果上面有，现在在每个里面完成。 //left_speed=0 //right_speed=0 //gosub发送电机命令 其他的 /*HH2！ 2*///或继续上一命令 left_speed=温度left_seed 右侧速度=时间右侧速度 //gosub发送电机命令//gb 23.10.08 结尾 /*HH2！ 2*/no_avoidance: gosub发送电机命令//gb 23.10.08 old_x_wc=x_wc 旧_y_wc=y_wc old_phi_wc=phi_wc //也用于预测和声纳绘图。。。 /巴基斯坦 /*SH2！ 2*/endwhile //巴基斯坦 //int bla=获取（） gosub停止_电机 /---------------------------------------------- 返回 /*SH1！ 1*/ /*HS！！ 1*/prefect_current_shaft_position://延迟可调 如果模拟=1 返回 结尾 浮动延迟1=1.20 float time_delay_shaft=经过的时间（1）-最后一次时间轴 //0.94=与RugW同步发送回值的次数（秒）。 //之前的0.6、0.5可以，0.42、0.695 //打印“---------------”，时间_播放_轴 //注：我假设delay1+delay2（下一个子例程）=总时间延迟 //在威康大约2.5秒 浮点local_x、local_y、local_phi d_x轴_dt=延迟1*（x轴-旧x轴）/时间延迟轴 d_y_shaft_dt=延迟1*（y_shaft-旧y_shave）/时间延迟轴 角度1=old_phi_shaft 角度2=phi_shaft 戈苏布角_diff d_phi_shaft _dt=延迟1*（角度差异）/时间延迟_shaft /d_phi_shaft _dt=延迟1*（phi_shaft-旧的高速轴）/时间延迟轴 浮子delta_s=sqrt（d_x_shaft_dt*d_x_shaft_dt+d_y_shaft_dt*d_y_shaft_dt） 局部x=x轴+增量s*cos（phi轴+0.5*d_phi轴dt） local_y=y_shaft+delta_s*sin（phi_shaft+0.5*d_phi_shaft_dt） local_phi=phi_shaft+d_phi_shaft_dt old_x_shaft=x轴 old_y_shaft=y_shaft old_phi_shaft=phi_shaft x_轴=局部_x y轴=本地y phi_shaft=本地phi /win_move本地_x，本地_y last_time_shaft=经过的时间（1） 返回 /*HH1！ 1*/prefect_position_of_control_action://延迟可调 如果模拟= 1 浮动延迟2=1.3//1.75 float time_delay_motor=经过的时间（1）-最后一次时间电机 //前1.5 //打印“----------------”，time_delay_motor 浮点local_x、local_y、local_phi d_x_wc_dt=延迟2*（x_wc-旧x_wc）/时间延迟电机 d_y_wc_dt=延迟2*（y_wc-旧y_wc）/时间延迟电机 d_phi_wc_dt=延迟2*（phi_wc-旧_高_wc）/时间_电机 /old_x_wc=x_wc /old_y_wc=y_wc /old_phi_wc=phi_wc 浮点增量s=sqrt（d_xwc_dt*d_xwc _dt+d_ywc _dt*d_yw _dt） local_x=x_wc+delta_s*cos（phi_wc+0.5*d_phi_wc_dt） 本地y=y_wc+delta_s*sin（phi_wc+0.5*d_phi_wc_dt） local_phi=phi_wc+d_phi_wc_dt x_wc_predict=本地x y_wc_predict=本地 phi_wc_predict=local_phi /win_move本地_x，本地_y /printf“预测%f，%f，%f\n”，x_wc_predict，y_wc_predict last_time_motor=经过的时间（1） else//在模拟模式下 x_wc_predict=x_wc y_wc_预测=y_wc phi_wc_predict=phi_wc 结尾 返回 /*HH121*/声纳安全： 如果ON_BRICKS=1 返回 结尾 //这个例行程序会停止轮椅，直到障碍物移动。 //如果1200毫秒后障碍物仍然存在，则控制 //返回到主循环，该循环将执行逃逸运动。 int轴编码器保持=0 int alarm_counter=0 /*HS！！ 2*/检测_障碍物： 前障碍=0 left_obstacle=0 右侧障碍=0 //对于声纳=4到6 //gosub快速获取onar_range //提前获取新数据 //如果声纳范围<150&声纳范围>0 //前障碍=1 //打印“FRONT=”，声纳，“range=”，sonar_range[sonar] //巴基斯坦 /转到sonar_stop //结尾 //下一个声纳 //主意，不是为了避免杂散的回声而一个接一个地做所有前面的声纳吗？ 声纳=4 gosub快速获取onar_range //提前获取新数据 如果声纳范围<150&声纳范围>0 前障碍=1 结尾 声纳=2 gosub快速设置sonar_range 声纳=3 gosub快速获取onar_range 声纳=5 gosub快速设置sonar_range //提前获取新数据 如果声纳范围<150&声纳范围>0 前障碍=1 结尾 声纳=7 gosub快速获取onar_range 声纳=8 gosub快速获取onar_range 声纳=6 gosub快速获取onar_range //提前获取新数据 如果声纳范围<150&声纳范围>0 前障碍=1 结尾 如果声纳范围[2]<80&声纳范围[2]>0 left_obstacle=1 //打印“声纳2:”，声纳范围[2]，“声纳3:”，声纳范围[3] /转到sonar_stop 结尾 如果sonar_range[8]<80&sonar_range[8]>0 右侧障碍=1 /转到sonar_stop 结尾 如果声纳范围[3]<60&声纳范围[3]>0 left_obstacle=1 /转到sonar_stop 结束语 如果sonar_range[7]<60&sonar_range[7]>0 右侧障碍=1 /转到sonar_stop 结尾 如果front_obstacle=1 | left_obstale=1 | right_obstable=1 转到声纳停止 结尾 如果轴编码器保持=1 gosub重置轴位置 //gosub复位陀螺仪 结束语 //打印“无障碍：安全” 返回 /-------------------------------- /*HH2！ 2*/声纳停止： 报警计数器+=1 left_speed=0 右侧速度=0 gosub发送电机命令 //我的声音9000100 打印“声纳停止”，“障碍物L F R=”，left_obstacle，“”，front_obstale，“” 消息_播放1200 如果轴编码器保持=0 old_x_wc=x_wc old_y_wc=y_wc old_phi_wc=phi_wc 轴编码器保持=1 结尾 如果报警计数器>=2 如果轴编码器保持=1 gosub重置轴位置 gosub复位陀螺仪 结尾 time_since_last_alarm=过去的时间（1）-最后的报警时间 如果timesince_last_alarm<20 报警持续时间+=1 其他的 报警持续时间=0 结束语 time_of_last_alarm=经过的时间（1） 返回 结尾 转到detect_an_coubler 返回 /*HH121*/check_sonar_validity: 浮子测试_phi=phi_shaft-0.45*d_phi_shaft_dt /在程序中，我们仍然使用之前的phiwc值。 /因此，测试值 浮子角度公差=12*PI/180 //该公差应取决于角速度。 //速度快时，测量点少 //公差应该增加。 浮点xy_tolerance=40 w_x_sonar=0 w_y声纳=0 w_phi_sonar=0 x_sonar=0 y_声纳=0 phi_sonar=0 found_flag=0 角度1=0 角度2=测试_phi 戈苏布角_diff /*SS！！ 2*/如果abs（angle_difference）<angle_tolerance //情况φ=0； 看窗帘：sonard2 如果abs（sonard2-max_room_ycm+y_wc）<xy_tolerance&sonar_range[3]>0 y_声纳+=最大房间ycm-声纳d2 phi_sonar=0 w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音2000100 结尾 如果abs（声纳d1-xwc）<xy公差&声纳范围[1]>0 x_sonar+=声纳_d1 phi_sonar=0 w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音2000100 结尾 如果abs（sonard3-max_room_xcm+x_wc）<xy公差和sonar_range[5]>0 x_sonar+=最大房间xcm-声纳_d3 phi_sonar=0 w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音2000100 结尾 结尾 /*HS！！ 2*/如果abs（angle_difference-0.5*PI）<angle_tolerance //案例φ=PI/2； 看看最近的边墙，或者都是d2左，d4右 如果abs（sonar_d4-max_room_xcm+x_wc）<xy_tolerance&sonar_range[7]>0 x_sonar+=最大房间xcm-声纳d4 phi_sonar=0.5*PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音3000100 结尾 如果abs（声纳d2-xwc）<xy公差和声纳范围[3]>0 x_sonar+=声纳d2 phi_sonar=0.5*PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音3000100 结尾 如果abs（sonard1-ywc）<xy公差&sonarrange[1]>0 y_声纳+=声纳d1 phi_sonar=0.5*PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音3000100 结尾 如果abs（sonard3-max_room_ycm+y_wc）<xy_tolerance&sonar_range[5]>0 y声纳+=（最大房间ycm-声纳d3） phi_sonar=0.5*PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音3000100 结束语 结尾 /*HH2！ 2*/如果abs（abs（angle_difference）-PI）<angle_tolerance //案例φ=PI； 看窗帘d4 如果abs（sonard4-max_room_ycm+y_wc）<xy_tolerance&sonar_range[7]>0 y_sonar+=最大oom_ycm-声纳_d4 phi_sonar=PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音5000100 结尾 如果abs（sonard2-ywc）<xy公差&sonarrange[3]>0 y_声纳+=声纳d2 phi_sonar=PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音5000100 结尾 如果abs（声纳d3-xwc）<xy公差&声纳范围[5]>0 x_sonar+=声纳d3 phi_sonar=PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音5000100 结尾 如果abs（sonard1-max_room_xcm+x_wc）<xy_tolerance&sonar_range[1]>0 x_sonar+=最大房间xcm-声纳_d1 phi_sonar=PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音5000100 结尾 结束语 /*HH2！ 2*/如果abs（angle_difference+0.5*PI）<angle_tolerance //案例φ=-PI/2； 看边墙d4（小x）和d2大x 如果abs（声纳d4-xwc）<xy公差&声纳范围[7]>0 x_sonar+=声纳d4 phi_sonar=-0.5*PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音4000100 /打印“Phi_wc=”，Phi_wc，“Phi_sonar=”，Phi_sonar /巴基斯坦 结尾 如果abs（sonard2-max_room_xcm+x_wc）<xy公差和sonar_range[3]>0 x_sonar+=最大房间xcm-声纳d2 phi_sonar=-0.5*PI w_x_sonar+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音4000100 结尾 如果abs（sonard1-max_room_ycm+y_wc）<xy_tolerance&sonar_range[1]>0 y_声纳+=最大房间ycm-声纳d1 phi_sonar=-0.5*PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音4000100 结尾 如果abs（sonard3-ywc）<xy公差&sonarrange[5]>0 y_声纳+=声纳d3 phi_sonar=-0.5*PI w_y声纳+=0.25 w_phi_sonar+=0.25 found_flag+=0.25 /我的声音4000100 结束语 结尾 /*HH2！ 2*///平均值 如果w_x_sonar>0 x声纳=0.25*x声纳/wx声纳 结尾 如果w_y_sonar>0 y声纳=0.25*y声纳/w_y声纳 结尾 w_x_sonar=0.8*w_x_sonar w_y声纳=0.8*w_y声纳 w_phi_sonar=0.8*w_phi_sonar /打印“ywc=”，y_wc，“ysonar=”，y声纳 返回 /*HH121*/重新校准： 打印“重新定位” 报警持续时间=0 /*HS！！ 2*/如果sonar_d2<120&sonar_range[3]>0 //需要左转 角度2=phi_wc 角度1=0 戈苏布角_diff 如果abs（angle_difference）<PI/5 y_shaft=最大房间ycm-100 y_声纳=y_轴 w_y声纳=1 w_y_轴=1 返回 结尾 如果abs（angle_difference）-0.5*PI<PI/5 x轴=100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结尾 如果abs（angle_difference）+0.5*PI<PI/5 x轴=最大房间xcm-100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结尾 如果abs（angle_difference）-PI<PI/5 y轴=100 y_声纳=y_轴 w_y声纳=1 w_y_轴=1 返回 结尾 返回 结尾 /*HH2！ 2*/如果sonar_d4<120且sonar_range[7]>0 //需要右转 角度2=phi_wc 角度1=0 戈苏布角_diff 如果abs（angle_difference）-PI<PI/5 y_shaft=最大房间ycm-100 y_声纳=y_轴 w_y声纳=1 w_y轴=1 返回 结尾 如果abs（angle_difference）-0.5*PI<PI/5 x轴=最大房间xcm-100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结束语 如果abs（angle_difference）+0.5*PI<PI/5 x轴=100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结尾 如果abs（angle_difference）<PI/5 y轴=100 y_声纳=y_轴 w_y声纳=1 w_y_轴=1 返回 结尾 返回 结尾 /*HH2！ 2*/如果sonar_d3<120且sonar_range[5]>0 //朝墙走去 角度2=phi_wc 角度1=0 戈苏布角_diff 如果abs（angle_difference）-0.5*PI<PI/5 y_shaft=最大房间ycm-100 y_声纳=y_轴 w_y声纳=1 w_y轴=1 返回 结尾 如果abs（angle_difference）<PI/5 x轴=最大房间xcm-100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结尾 如果abs（angle_difference）-PI<PI/5 x轴=100 x_sonar=x轴 w_x_sonar=1 w_x轴=1 返回 结尾 如果abs（角度差）+0.5*PI<PI/5 y轴=100 y_sonar=y_轴 w_y声纳=1 w_y轴=1 返回 结尾 返回 结束语 返回 /*SH1！ 1*/ /*SS！！ 1*/calculate_new_motor_command: float next_x=节点输出（第一个节点（下一个位置）） float next_y=节点输出（first_node（next_Position）+1） float next_phi=节点输出（first_node（next_Position）+2） win_fillellipse next_0.1*l_arrow、next_y-0.1*l_arrow、next_x+0.1*l_arw、next_y+0.1*l _箭头、红色 win_move下一个x，下一个y win_line next_x+l_arrow*cos（next_phi），next_y+l_artow*sin（nexd_phi），黄色 /*HS！！ 2*///下一个点作为目标。 phi2=与目标的角度 浮点dx=下一个x-x wc float dy=下一个y-y_wc 浮点分布=sqrt（dx*dx+dy*dy） phi2=999 如果dx=0&dy>0 phi2=PI/2 结尾 如果dx=0&dy<0 phi2=-PI/2 结尾 phi2=反正切（dy/dx） 如果dy<0&dx<0 phi2=phi2+PI 结尾 如果dy>=0&dx<0 phi2=phi2+PI 结尾 如果dy<0&dx>0 phi2=phi2+2*PI //避免所有负角 结尾 //phi2是机器人应该指向目标的地方 /*HH2！ 2*///----当前机器人航向phi_wc和期望的一个phi2之间的差异 /尝试在动作开始时使用预测的角度： /不是一个好主意，预测有很大的跳跃 /记录声纳数据时 // //浮点phip=phi_wc_predict //而不是phiwc float sind=（cos（phi2）*sin（phi_wc）-sin（phi2）*cos（phi_wc）） 浮点cosd=（cos（phi2）*cos（fiwc）+sin（phi2 float local_tan=sind/cosd 浮点delta_phi=arctan（local_tan） 如果sind>=0&cosd<0 delta_phi+=PI 结尾 如果sind＜0&cosd＜0 delta_phi=PI 结尾 delta_phi=-增量 /*SH2！ 2*//----速度控制------新-威康信托 基本速度=区段速度[当前区段] 浮动距离0=1//m 浮点w1=1/（1+exp（-（0.01*dist-dist0））//距离（单位：m），用于通过与目标的角度进行控制 浮点w2=1/（1+exp（+（0.01*dist-dist0））//以m为单位的距离，用于通过航向差异（delta_phi）进行控制 //距离效应 //打印“w1=”，w1，“w2=”，w2 角度1=phi_wc 角度2=next_phi 戈苏布角_diff //浮点exp2=exp（-0.5*（angle_difference）*（angel_difference）） //方向效应 浮动速度=0.5*基本速度*（1+exp（-abs（delta_phi）） //所有关于根据角度差异调整速度 如果phi2=999 速度=0 结尾 //打印“base_speed=”，base_speed，“速度=”，速度 //浮点exp3=exp（-0.5*delta_phi*delta_phi） 浮点sp_gain=1.1//1都允许过多偏移//0.82+0.9*（exp3） 浮点vl1=2*速度/（1+exp（+sp_gain*delta_phi）） 浮点vr1=2*速度/（1+exp（-sp增益*增量）） 浮点vl2=2*速度/（1+exp（+sp_gain*angle_difference）） 浮动vr2=2*速度/（1+exp（-sp增益*角度差异）） 打印“vl1=”、vl1、“vr1=”，vr1 打印“vl2=”、vl2、“vr2=”，vr2 left_speed_desired=fmax（1，w1*vl1+w2*vl2） right_speed_desired=fmax（1，w1*vr1+w2*vr2） /浮子sp_gain=0.5+0.3*速度/15 /浮子sp_mom=0.2 /left_speed_desired=sp_mom*left_seed_dired+（1-sp_mon）*速度/（1+exp（+sp_gain*（delta_phi）） /right_speed_desired=sp_mom*right_seed_desired+（1-sp_mon）*速度/（1+exp（-sp增益*（delta_phi）） /浮子λ=0.4 //旧的良好价值观 //---------- ///浮动速度=0.7*基本速度+0.3*基本速度*（exp1*exp2*exp3） ///浮子sp_gain=0.82+0.9*（exp3） 浮子sp_mom=0.2//0.22+（0.8/速度） left_speed_desired=（1-sp_mom）*left_seed_dired+sp_mom*浮点（left_sepe） right_speed_desired=（1-sp_mom）*right_seed_dired+sp_mom*浮点（right_sspeed） //浮动λ=0.54*sp_增益 //晚上看起来很有希望 //浮子λ=0.5//gbtest 090712 //----------- left_speed=int（left_seed_dired） right_speed=int（右速度所需） 打印“Ldes=”，left_speed_desired，“Rdes.=”，right_speed_desired，”phi_wc=“，180*phi_wc/PI ///left_speed=int（left_seped_directed+lambda*（left-speed_direct-float（left_speed）） ///right_speed=int（right_seped_desired+lambda*（right-speed_desided-float（right_speed）） /*SS！！ 2*/返回 /*HS！！ 1*/send_motor_command://此处为电机偏置。 //int方向字节=0 //如果right_speed_desired>=0 //方向字节+=1 //结尾 //如果left_speed_desired>=0 //方向字节+=2 //结尾 //包括向后运动 //RugWar董事会考虑 消息字节=107 gosub握手发送字节 //速度不得超过+-100 message_byte=int（0.80*float（left_speed））+128 //Wellcome的0.8分是0.85分 //message_byte=int（1.1*float（left_speed））+128 //系数1.1补偿了左右不对称 //在Penny and Gilles系统中。 //现在（2009年11月）似乎是另一种情况，右轮迟缓。 gosub握手发送字节 //message_byte=右侧速度+128 message_byte=int（1.15*float（right_speed））+128 gosub握手发送字节 //message_byte=方向字节 //gosub握手发送字节 //包括向后运动 //RugWar董事会考虑 打印“发送L=”，left_speed，“R=”，right_speed 返回 /*HH1！ 1*/停止电机： 消息字节=99 gosub握手发送字节 返回 /*SH1！ 1*/ /*HS！！ 1*/get_gyro_data（获取日期）： 消息字节=105 gosub握手发送字节 gosub获取float_data phi_gyro=1.26*PI*浮动数据/180 /打印“phi_陀螺=”，180*phi_陀螺/PI 返回 /*HH1！ 1*/重置陀螺仪： /gb 23.02.03：未使用陀螺仪。 /消息字节=125 /gosub握手发送字节 / /float_data=180*phi_wc/PI /gosub发送float_data 返回 /*HH1！ 1*/获取电池数据： 整数b3，b2，b1 消息字节=106 gosub握手发送字节 gosub获取float_data 返回 /*HH1！ 1*/获取轴位置数据： 整数b3，b2，b1 消息字节=100 gosub握手发送字节 //调试 //gosub获取float_data //float rr=浮点数据 //gosub获取float_data //float ll=浮点数据 //结束调试 gosub获取float_data x_shaft=浮动数据 /巴基斯坦 gosub获取float_data y_shaft=浮动数据 /巴基斯坦 gosub获取float_data phi_shaft=PI*float_data/180 /打印“x轴=”，x轴，“，y轴=” 返回 /*HH1！ 1*/重置轴位置： 消息字节=120 gosub握手发送字节 float_data=x_wc gosub发送float_data float_data=y_wc gosub发送float_data float_data=180*phi_wc/PI gosub发送float_data 返回 /*HH1！ 1*/重置轴位置_零： x_wc=0 y_wc=0 gosub重置_轴位置 返回 /*SH1！ 1*/ /*HS！！ 1*/float_to_tree_bytes: b3=0 int Rest，Counter_base=128 当float_data>320.00时 float_data=32000.0 b3+=1 循环结束 当float_data<-32000.0时 float_data+=32000.0 b3+=1 循环结束 Rest=int（float_data） 如果float_data<0.0 Rest=-int（float_data） b3=b3+计数器基础 结尾 b1=土地（Rest，255） b2=休息/256 //打印“---b3=”，b3，“b2=”，b2，“b1=” 返回 /*HH1！ 1*/获取浮动数据： gosub握手_读取字节 b3=以字节为单位 gosub握手读取字节 b2=以字节为单位 gosub握手_读取字节 b1=英寸字节 如果b3<128 float_data=32000*浮点（b3）+256*浮点（b2）+浮点（b1） 结尾 如果b3>=128 float_data=-32000*浮点（b3-128）-256*浮点（b2）-浮点（b1） 结尾 //打印“---b3=”，b3，“b2=”，b2，“b1=”，B11，“-->Float_data=”，Float_data 返回 /*HH1！ 1*/发送浮动数据： gosub浮点到三字节 消息字节=b3 gosub握手发送字节 消息字节=b2 gosub握手发送字节 消息字节=b1 gosub握手发送字节 返回 /*HH1！ 1*/握手发送字节： int in_byte，serial_count=0 /打印“” in_byte=串行字节输入（端口） //只是清空可能等待okbyte的缓冲区 发送字节循环： 序列计数+=1 串行字节输出端口，消息字节 in_byte=串行字节输入（端口） /打印f“%d，”，单位为字节 if（in_byte！=ok_byte）&（serial_count<serial_cont_limit） 转到send_byte_loop 结尾 如果serial_count>=serial_cont_limit 打印“” 打印“发送时RugWarrior没有响应。” 打印“请按ESC，重置RugWarrior板。” 哔哔声 巴基斯坦 结束语 返回 /*HH1！ 1*/握手_读取字节： int in_byte，in_byte1，serial_count=0 /打印“” 读_循环： 序列计数+=1 in_byte=serial_byte_in（端口） in_byte1=串行字节输入（端口） /打印f“%d-%d，”，以字节为单位，以字节1为单位 如果（（in_byte=300）&（serial_count<serial_cont_limit））|（in_byte1！=in_byte） 转到读取操作 结尾 /打印f“->%d，”，单位为字节 如果serial_count>=serial_cont_limit 打印“” 打印“阅读时RugWarrior没有回应。” 打印“请重置并检查电缆或电源。” 哔哔声 巴基斯坦 结束语 串行字节输出端口，ok_byte 返回 /*SH1！ 1*/ /*HS！！ 1*/内部： //dg 20,15，标准代码类型 dg 40,30，标准代码类型 //dg 54,40，标准代码类型 mg对比度 grid_nodestyle对比度，GREYSC_DISP_STYLE 牵引网对比度 mg霍夫 grid_nodestyle Hough，灰色_显示_样式 牵引格栅Hough dg 8,3，标准代码类型 mg平均值 grid_nodestyle平均值，GREYSC_DISP_STYLE 牵引网平均值 返回 /*HH121*/build_trachicles://新的包括圆直径+元素数 浮点x1，y1，phi1，x1next，y1next，phi1next 浮点θ，θ_下一个，r int节点id /*HS！！ 2*///神经网络大小定义 /两个圆 截面尺寸[1]=12 截面尺寸[2]=12 截面尺寸[3]=12 截面尺寸[4]=12 /向内螺旋 截面_尺寸[5]=12 截面尺寸[6]=10 截面尺寸[7]=8 截面尺寸[8]=5 截面尺寸[9]=5 /四个八分之一 /截面尺寸[10]=6 /截面_尺寸[11]=6 /截面尺寸[12]=6 /截面尺寸[9]=6 截面尺寸[10]=6 截面尺寸[11]=6 截面尺寸[12]=6 截面尺寸[13]=6 截面尺寸[14]=6 截面尺寸[15]=6 截面尺寸[16]=6 截面尺寸[17]=6 截面尺寸[18]=6 截面尺寸[19]=6 截面尺寸[20]=6 截面尺寸[21]=6 /向外螺旋 /截面尺寸[26]=3 /截面尺寸[27]=4 截面尺寸[22]=6 截面尺寸[23]=8 截面_尺寸[24]=10 截面尺寸[25]=12 int网络大小=0 //i=1至31 //对于i=1到4//31:1->4=仅对两个外圆进行编码。 阿姆斯特丹 /net_size+=节大小[i] //下一个i //WELLCOME TRAJECTORY公司 i=1到31//威康 如果节列表[i]=- 99 net_size+=节大小[节列表[i]] 结束语 下一个i /*HH2！ 2*//nn版本 dg 1,2，输入代码类型 mg位置 grid_nodestyle位置，GREYSC_DISP_STYLE 拉线格栅位置 dg 12，int（浮点（net_size）/12）+1，GRBF_NODE_TYPE mg轨迹 grid_nodestyle轨迹，GREYSC_DISP_STYLE 牵引网轨迹 dg 1，1，线性_代码_类型 mg序列_状态 grid_nodestyle Sequence_State，灰色_显示_样式 drawgrid序列_状态 /dg 1，1，线性_代码_类型 mg特制2 grid_nodestyle Special2，灰色_显示_样式 特殊牵引网2 dg 1,4，线性代码类型 mg下一位置 grid_nodestyle Next_Position，灰色_显示_样式 绘图网格Next_Position 连接类型ALL_TO_ALL 连接位置，轨迹 连接轨迹，下一个位置 连接轨迹，序列状态 设置权重0 设置网格权重轨迹，999999 int节=1 int last_node=first_node（轨迹）-1 对于节点id=last_node+1到last_node+节大小[1] 一致节点Sequence_State，节点_id /set_weightsbetnodes first_node（Sequence_State）、nodes_id、section_boost*（float（section）-0.5） 下一个节点id last_node=last_node+节大小[1] 截面=2 对于nodes_id=last_node+1到last_node+section_size[2] congridnode Special2，节点_id /set_weightsbetnodes first_node（Special2）、nodes_id、section_boost*（float（section）-0.5） /基于旧节 set_weightsbetnodes first_node（Special2）、nodes_id、section_boost*（float（nodes_id=first_node（Trajective）+1）-0.5） /基于新节点 下一个节点id last_node=last_node+section_size[2] 连接Sequence_State，特殊2 set_weightsbetgrids Sequence_State，特殊2，1 对于节点_id=last_node+1到first_node（轨迹）+网络大小-1 一致节点Sequence_State，节点_id 下一个节点id 超宽带 /*SH2！ 2*///轨迹尺寸+边距 //r=350-低增益引起的误差边缘 /浮动裕度=100 /画廊设置 浮动裕度=50 /阿姆斯特丹09和Wellcome 2012的设置 /浮动裕度=30 /图形设置 浮子r0=300//200：一个直径为6m的圆，用于Wellcome Trust float dist_from_back_wall=150//未使用？ 浮点数xc=0.5*max_room_xcm//圆心 //float yc=max_room_ycm-dist_from_back_wall-r-边距 浮子yc=0.5*max_room_ycm+50//圆心 //浮动yc=最大值_room_ycm-xc//iniva //float yc=410//阿姆斯特丹 /*嘘！！ 2*//第一个1/2圆，第1段 last_node=first_node（轨迹）-1 截面=1 浮点start_angle=PI 浮点旋转dir=1 //1=逆时针 //相反（顺时针）=-1 r=r0-边缘-10 xc=0.5*最大体积xcm /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第二个1/2圈，第2段 截面=2 start_angle=0 //相反（顺时针）=-1 r=r0-边缘-10 xc=0.5*最大房间xcm /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第三个1/2圈，第3段 截面=3 start_angle=PI //相反（顺时针）=-1 r=r0-边缘-10 xc=0.5*最大房间xcm /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第四个1/2圈，第4段 截面=4 启动角度=0 //相反（顺时针）=-1 r=r0-边缘-10 xc=0.5*最大体积xcm /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//向内螺旋，第5节 截面=5 start_angle=PI //相反（顺时针）=-1 r=（5/6）*（r0-边距） xc=0.5*最大房间xcm-（0.5/6）*（r0-边距） /*SH2！ 2*///gosub编码器_半圆形 /*HS！！ 2*//向内螺旋，截面6 截面=6 start_angle=0 //相反（顺时针）=-1 r=（4/6）*（r0-边距） xc=0.5*max_room_xcm+（0.25/6）*（r0裕度） /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//螺旋向内，第7节 截面=7 start_angle=PI //相反（顺时针）=-1 r=（3/6）*（r0-边距） xc=0.5*最大房间xcm-（0.5/6）*（r0-边距） /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//向内螺旋，截面8==8 截面=8 start_angle=0 //相反（顺时针）=-1 r=（2.2/6）*（r0-余量） xc=0.5*最大房间xcm /*SH2！ 2*//gosub编码器_半圆形 /*HS！！ 2*//---螺旋向内，第9节 截面=9 start_angle=PI //相反（顺时针）=-1 r=（1/6）*（r0-边距） xc=0.5*最大房间xcm-（0.5/6）*（r0-边距） /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//----图8，第10节 截面=10 start_angle=PI rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*//gosub编码_半圆形 /*HS！！ 2*//---第11节第8张图 截面=11 start_angle=0 rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//---第12节第8张图 截面=12 start_angle=0 rotation_dir=1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第13节第8张图 截面=13 start_angle=PI rotation_dir=1 //相反（顺时针）=-1 r=（1/2）*（r0-边距） xc=0.5*最大房间xcm-r //r=（1/6）*（r0-边距） //xc=0.5*max_room_xcm-r-（0.25/6）*（r0裕度） /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第14节第8张图 截面=14 start_angle=PI 旋转_方向=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第15节第8张图 截面=15 start_angle=0 rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第16节第8张图 截面=16 start_angle=0 旋转_方向=1 //相反（顺时针）=-1 r=0.5*（r0裕度） xc=0.5*最大房间xcm-r /*SH2！ 2*/gosub编码_半圆 /*HS！！ 2*//第17节第8张图 截面=17 start_angle=PI rotation_dir=1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第18节第8张图 截面=18 start_angle=PI rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第19节第8张图 截面=19 start_angle=0 rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第20节第8张图 截面=20 start_angle=0 rotation_dir=1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第21节第8张图 截面=21 启动角度=PI rotation_dir=1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*嘘！！ 2*//第22节第8张图 截面=22 start_angle=PI rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//第23节第8张图 截面=23 start_angle=0 rotation_dir=-1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm+r /*SH2！ 2*///gosub编码_半圆 /*嘘！！ 2*//第24节第8张图 截面=24 start_angle=0 rotation_dir=1 //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//图8，第25节 截面=25 start_angle=PI //相反（顺时针）=-1 r=0.5*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码器_半圆形 /*HS！！ 2*//--螺旋向外，第26节 截面=26 start_angle=0 //相反（顺时针）=-1 r=0.5*（1/4）*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*//gosub编码_半圆形 /*HS！！ 2*//---螺旋向外，第27节 截面=27 start_angle=PI //相反（顺时针）=-1 r=0.5*（（1/4）+（1/3））*（r0-余量） xc=0.5*最大房间xcm /*HH2！ 2*//gosub编码器_半圆形 /*HH2！ 2*//螺旋向外，截面28==22 截面=28 start_angle=0 //相反（顺时针）=-1 r=（2/7）*（r0-边距） /r=0.5*（（2/4）+（1/3））*（r0-余量） xc=0.5*最大房间xcm-r /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*//向外螺旋，截面29==23 截面=29 start_angle=PI //相反（顺时针）=-1 r=0.5*（（2/4）+（2/3））*（r0-余量） xc=0.5*最大房间xcm /*SH2！ 2*///gosub编码_半圆 /*嘘！！ 2*//向外螺旋，截面30==24 截面=30 start_angle=0 //相反（顺时针）=-1 /r=（4/6）*（r0-边距） r=0.5*（（3/4）+（2/3））*（r0-余量） xc=0.5*最大房间xcm-（0.5/6）*（r0-边距） /*HH2！ 2*///gosub编码_半圆 /*HH2！ 2*//螺旋向外，截面31==25 截面=31 start_angle=PI //相反（顺时针）=-1 //新的1/2圆 //r=r0-边缘-10 //xc=0.5*最大房间xcm //旧的1/2螺旋输出 r=0.5*（（3/4）+（3/3））*（r0-余量） xc=0.5*max_room_xcm+（0.25/6）*（r0裕度） /*SH2！ 2*///gosub编码_半圆 /*HS！！ 2*///设置倒数权重（轨迹<->Sequence_State） int节，last_node=first_node（轨迹）-1 浮点previous_phase=0，current_phase=0 //截面=1至31 //截面=1至4//31//只有2个外圆。 阿姆斯特丹 i=1至31 节=节列表[i] if部分=- 99 对于nodes_id=last_node+1到last_node+section_size /float node_sigma=GRBF_node_sigma（节点id） /previous_phase=线权重（线间节点（节点id，第一个节点（序列状态））） /current_phase=前一个阶段+节点_西格玛*节_ boost*浮点（节点_id-第一个节点（轨迹）+1） /set_weightsbetnodes节点id、first_node（Sequence_State）、current_phase /基于全新节点+西格玛 set_weightsbetnodes nodes_id，first_node（Sequence_State），section_boost*float（nodes_id-first_node（Trajectory）+1） /基于新节点 /set_weightsbetnodes节点id、first_node（Sequence_State）、section_boost*float（section） /基于旧节 if节！= 2 //上述第2节 /current_phase=前一个阶段+节点_西格玛*截面_ boost*（浮点（节点_id-第一个节点（轨迹）+1）-0.5） /set_weightsbetnodes first_node（Sequence_State）、nodes_id、current_phase /基于全新节点+西格玛 /set_weightsbetnodes first_node（Sequence_State）、nodes_id、section_boost*（float（section）-0.5） /基于旧节 set_weightsbetnodes first_node（Sequence_State）、nodes_id、section_boost*（float（nodes_id=first_node（Trajective）+1）-0.5） /基于新节点 结尾 下一个节点id last_node=last_node+节大小 结尾 //打印“section done=”，section，“last_node=”，last_node //巴基斯坦 下一个i //下一节 返回 /*HH1！ 1*/encode_half_circle://此处设置新：sigma /如果节>0 i=0到section_size -1 θ=start_angle+rotation_dir*float（i）*PI/float（截面尺寸 ) theta_next=start_angle+rotation_dir*浮点（i+1）*PI/浮点（截面尺寸 ) x1=r*cos（θ）+xc y1=r*sin（θ）+yc phi1=θ+PI/2 x1next=r*cos（theta_next）+xc y1next=r*sin（theta_next）+yc phi1next=theta_next+（PI/2）*（rotation_dir）//gb /打印“Theta=”，Theta /巴基斯坦 节点id=最后节点+i+1 set_weightsbetnodes first_node（位置），节点id，x1 set_weightsbetnodes first_node（位置）+1，节点id，y1 set_weightsbetnodes nodes_id，first_node（下一个位置），x1next 集合权重集合节点节点id，第一个节点（下一个位置）+1，y1next //设置权重设置节点节点id，第一个节点（下一个位置）+2，phi1next set_weightsbetnodes节点id，第一个节点（Next_Position）+2，sin（phi1next） set_weightsbetnodes节点id，第一个节点（Next_Position）+3，cos（phi1next） //打印“节点=”，节点id /打印i，“，”，180*phi1next/PI //巴基斯坦 /set_GRBF_node_sigma节点id，max_room_xcm/15 //set_GRBF_node_sigma节点id，25+r/5//阿姆斯特丹 set_GRBF_node_sigma节点id，25+r/10/wellcome /用于现场绘图 /set_GRBF_node_sigma节点id，20+r/5 /用于绘制轨迹 /set_GRBF_node_sigma节点id，r/3 /设置GRBF_node_sigma节点id， /float（节点id-第一个节点（轨迹）+1） 下一个i last_node=节点id //打印“section done=”，section，“last_node=”，last_node //巴基斯坦 /结尾 返回 /*HH1！ 1*/建筑_圆形： 返回 /*HH121*/update_network2://新增。 处理结束循环 set_node_output first_node（位置），x_wc_predict set_node_output first_node（位置）+1，y_wc_predict /set_node_output first_node（位置）+2，phi_wc_predict updategrid轨迹 int current_grid=轨迹 gosub规范化_内部 /gosub侧向抑制 gosub update_Sequence_State（gosub更新序列状态） /注释掉用于现场绘图 current_section=int（node_output（first_node（Sequence_State））/section_boost） 打印“====>Section nb=”，node_output（first_node（Sequence_State））/Section_boost，“ updategrid Special2 /*嘘！！ 2*///处理回第一状态的循环 //最后->切换到下一节之前的三个状态 //（解释下面的2.5和3） 如果node_output（first_node（Sequence_State））>=（float（net_size）-2.5）*section_boost //从最后一个州跳到第12个州。 旧版本。 //避开第1节。 原因不明。 //set_grid_outputs Special2，13*节_boost //set_grid_outputs Sequence_State，13*section_boost（设置网格输出序列状态） //试着跳到第一节的开头。 阿姆斯特丹09 forced_next_state=3-浮点（网络大小）+（节点输出（第一个节点（序列状态））/节boost） 打印“forced_next状态=”，forced_next_state //打印“当前状态=”，node_output（first_node（Sequence_state））/section_boost //巴基斯坦 set_grid_outputs特殊2，强制next_state*section_boost set_grid_outputs Sequence_State，强制next_State*section_boost 更新网格轨迹 /基于新节点 结尾 /打印“-----Special2=”，node_output（first_node（Special2）） gosub更新_文本_位置 /drawgrid轨迹、下一个位置、位置、序列状态、特殊2 //微克 返回 /*HH1！ 1*/update_Next_Position://新建 浮点act_sum=0 //注意：圆圈闭合时要小心，360=0（但av=180！） 更新网格Next_Position //巴基斯坦 对于k=first_node（轨迹）到last_node（轨迹） 让act_sum=act_sum+节点输出（k） //打印f“%2.20f，”，act_sum 下一个k //打印“act_sum=”，act_sum //printf“\n” /已在先前调用的update_Sequence_State中完成 如果act_sum>0.0 对于k=first_node（Next_Position）到last_node（Next_Position） //打印“节点=”，k，“节点输出=”，节点输出（k） 集合节点输出k，节点输出（k）/动作总和 下一个k //使用sin和cos计算进入第三个节点的下一个角度。 浮点sind=节点输出（第一个节点（下一个位置）+2） 浮点cosd=节点输出（第一个节点（下一个位置）+3） /打印“sind=”，sind，“cosd=”，cosd 浮点delta_phi=arctan（sind/cosd） 如果sind>=0&cosd<0 delta_phi+=PI 结尾 如果sind<0&cosd<0 delta_phi=PI 结束语 设置节点输出第一个节点（下一个位置）+2，增量 其他的 打印“低活动限制” /巴基斯坦 结束语 返回 /*HH1！ 1*/update_Sequence_State://新建 浮点act_sum=0 /float act1=节点输出（first_node（Sequence_State）） updategrid Sequence_State（更新网格序列状态） float act1=节点输出（first_node（Sequence_State）） 对于k=first_node（轨迹）到first_nde（轨迹）+net_size-1 让act_sum=act_sum+节点输出（k） 下一个k /巴基斯坦 k=第一节点（序列状态） /如果act_sum>0.000001 如果act_sum>0.0 集合节点输出k，节点输出（k）/动作总和 /我的声音100100 其他的 /巴基斯坦 集合节点输出k，act1 结束语 返回 /*HH1！ 1*/角度偏差： 浮点sind=（cos（角度2）*sin（角度1）-sin（角2）*cos（角1）） 浮点数cosd=（cos（角度2）*cos（角1）+sin（角度2 float local_tan=sind/cosd 浮点角度差异=arctan（local_tan） 如果sind>=0&cosd<0 角度差异+=PI 结尾 如果sind<0&cosd<0 角度差异-=PI 结尾 angle_difference=-angle_defference 返回 /*SH1！ 1*/ /*HS！！ 1*/get_sonar_data://用于自我定位-是否使用？ //graph_active声纳 //graph_clear（图形_清除） 键盘=端口输入（96） //注意：不知怎的，这个读数愚弄了节能系统 //就好像按下了一个键，再也没有减速了 //几秒钟后数据读数下降。 //端口96是键盘芯片8255A（十六进制60）。 //打印键盘 /消息_播放40 声纳=3 gosub快速获取onar_range /消息_播放40 声纳=1 gosub快速设置sonar_range /消息_播放40 声纳=5 gosub快速获取onar_range /消息_播放40 声纳=7 gosub快速获取onar_range /ms_播放40 /对于声纳=1到8 /gosub快速获取onar_range /下一个声纳 sonar_d1=声纳范围[1]-声纳y[1] sonar_d2=声纳范围[3]-声纳x[3] sonar_d3=声纳范围[5]+声纳y[5] sonar_d4=声纳范围[7]+声纳_x[7] 返回 /*HH1！ 1*/fast_get_sonar_range://ibm thinkpad //声纳范围[声纳]=200 //gosub get_sonar_range//gb测试 //返回 ms_delay 30//20。。。 3m时为10ms int掩码=2^（声纳-1） int echo_on=1 整数iix，iiy 端口_输出端口b，掩码 /====================== //ms_delay 1（毫秒延迟1） 端口_输出端口，掩码 对于i=0到max_sonar_array-1 回波[i]=0//gb 130811 如果echo_on=1 //echo[i]=端口in（端口b） iix=1到62//50//以在port_in调用之间创建延迟 iiy+=iix//ibm thinkpad需要 接下来iix//这个延迟也会改变阵列中回声的位置。 //用62 ittreaqtio和系数1.42，我们得到了正确的距离。。。 //用50 ittreaqtio和系数1.17，我们得到了正确的距离。。。 echo[i]=端口输入（端口b） 结尾 如果echo[i]=掩码&echo_on=1 echo_on=0 echo[i]=port_in（portb）//运行一次 结尾 如果echo_on=0 echo[i]=mask//模拟其余输入 结尾 //打印回声[i] 下一个i 消息_播放1 端口_输出端口，0 /===================== int i_dist=-1 对于i=min_range_check到max_sonar_array-1 如果echo[i]=掩码 如果i_dist=-1 i_dist=i i=最大sonar_array 结尾 结尾 下一个i 声纳范围[声纳]=浮标（i_dist）*1.42//1.17//1.19 //打印“声纳=”，声纳，“range=”，sonar_range[sonar] 返回 /*HH1！ 1*/声纳_扫描8： 而1 graph_active声纳 graph_clear（图形_清除） 键盘=端口_in（96） 对于声纳=1到8 //声纳=5 gosub获取sonar_range //gosub获取sonar_range_old //ms_delay 400//50 下一个声纳 循环结束 返回 /*HH1！ 1*/get_sonar_range://当接收到回声时，新测试停止读取回声，gb 27.6.2012 浮子dt //声纳=5//g测试 int掩码=2^（声纳-1） //int mask=2^（5-1）//gbtest仅从前面的5读取 int echo_on=1 内部iix，iiy ms_delay 30//gb 10ms=3m port_out端口b，掩码//读取地址 //消息_播放1 /初始化计时器 /====================== //int掩码=2^（sonar-1）//gbtest port_out端口，屏蔽//ping out 对于i=0到max_sonar_array-1 回波[i]=0//gb 130811 如果echo_on=1 //echo[i]=端口in（端口b） 对于iix=1到50//1//，在port_in调用之间创建延迟 iiy+=iix//ibm thinkpad需要 接下来iix//这个延迟也会改变阵列中回声的位置。 //利用61 ittreaqtio和系数1.386，我们得到了正确的距离。。。 //用62 ittreaqtio和系数1.42，我们得到了正确的距离。。。 //用50 ittreqtio和因子1.17，我们得到了正确的距离。。。 echo[i]=端口in（端口b） 结尾 如果echo[i]=掩码&echo_on=1 echo_on=0 echo[i]=port_in（portb）//再次运行 结尾 如果echo_on=0 echo[i]=掩码//以模拟其余输入 结尾 //打印回声[i] 下一个i 消息_播放1 端口输出端口，0//重置所有输入？ /===================== /dt=经过的时间（1） /----- int i_dist=0 /初始化计时器 对于i=min_range_check到max_sonar_array-1 颜色=绿色 /如果land（echo[i]，mask）=0 如果echo[i]=0 颜色=红色 结尾 /if land（echo[i]，mask）=掩码 如果echo[i]=掩码 颜色=黄色 如果i_dist=0 i_dist=i /i=最大sonar_array 结尾 结尾 移动浮标（i），浮标（声纳+1）-0.3 线浮标（i），浮标（声纳+1）+0.3，彩色 下一个i //打印“声纳”，声纳，“，range=”，i_dist，“（dt1=”，dt，“dt2=”，elapsed_time（1），“）” 打印“声纳”，声纳，“，范围[cm]=”，浮标（i_dist）*1.17//1.389 如果声纳=8 打印“” 结尾 sonar_range[sonar]=浮子（i_dist）*1.17//1.19 返回 /*HH1！ 1*/get_sonar_range_old: 浮子dt int掩码=2^（声纳-1） 整数iix，iiy 端口_输出端口b，掩码 /初始化计时器 /====================== 端口_输出端口，掩码 对于i=0到max_sonar_array-1 echo[i]=0//gb 130811 echo[i]=端口in（端口b） iix=1到10//在port_in调用之间创建延迟 iiy+=iix//ibm thinkpad需要 下一个iix //打印回声[i] 下一个i 端口输出端口，0 /===================== /dt=经过的时间（1） /----- int i_dist=0 /初始化计时器 对于i=min_range_check到max_sonar_array-1 颜色=绿色 /如果着陆（回波[i]，掩码）=0 如果echo[i]=0 颜色=红色 结尾 /if land（echo[i]，mask）=掩码 如果echo[i]=掩码 颜色=黄色 如果i_dist=0 i_dist=i /i=最大sonar_array 结尾 结尾 移动浮筒（i），浮筒（声纳+1）-0.3 线浮标（i），浮标（声纳+1）+0.3，彩色 下一个i //打印“声纳”，声纳，“，range=”，i_dist，“（dt1=”，dt，“dt2=”，elapsed_time（1），“）” 打印“声纳”，声纳，“，范围[cm]=”，浮标（i_dist）*1.389 如果声纳=8 打印“” 结尾 声纳范围[声纳]=浮标（i_dist）*1.19 //sonar_range[sonar]=浮点（i_dist）*1.19//旧笔记本电脑的值 //结尾 返回 /*HH1！ 1*/sonar_plot: /-------声纳图------------ //------------删除旧数据------------- //打印f“x_wc=%f，y_wc=%f，phi_wc=1%f\n”，x_wc，y_wc，phi_wc //打印“phi_wc=”，phi_wc*180/PI 颜色=0 //0=黑色 对于声纳=1到8 xs=old_x_wc+sonar_x[声纳]*cos（old_phi_wc-0.5*PI）-sonar_y[声纳]*sin（old_phi_wc-0.5m*PI） ys=old_y_wc+sonar_y[声纳]*cos（old_phi_wc-0.5*PI）+sonarx[声纳]*sin（old_phi_wc-0.5m*PI） win_移动xs，ys xs=xs+old_sonar_range[声纳]*cos（old_phi_wc-0.5*PI+sonar_dir[声纳） ys=ys+old_sonar_range[声纳]*sin（old_phi_wc-0.5*PI+sonar_dir[声纳]） win_line xs、ys、颜色 下一个声纳 //打印“擦除旧声纳” //巴基斯坦 //------------新数据------------- 颜色=5 //灰色等级 对于声纳=1到8 xs=x_wc+声纳_x[声纳]*cos（phi_wc-0.5*PI）-声纳_y[声纳]*sin（phi_wc-0.5*PI） ys=y_wc+sonar_y[声纳]*cos（phi_wc-0.5*PI）+sonar_x[声纳]*sin（phi_wc-0.5*PI） win_move xs，ys（冬季移动xs，ys） xs=xs+声纳范围[声纳]*cos（phi_wc-0.5*PI+声纳dir[声纳】） ys=ys+声纳范围[声纳]*sin（phi_wc-0.5*PI+声纳dir[声纳]） win_line xs，ys，颜色 旧声纳范围[声纳]=声纳范围[声纳] 下一个声纳 /巴基斯坦 返回 /*SH1！ 1*/ /*SS！！ 1*/ /*HS！！ 1*/查找最小最大值： 浮点max_act=0 浮动最小值=1 int最大节点=0，最小节点=0 对于nodes_id=first_node（current_grid）到last_node（current _ grid） 如果node_output（nodes_id）>max_act max_act=节点输出（节点id） max_node=节点id 结尾 如果node_output（nodes_id）＜min_act min_act=节点输出（节点id） min_node=节点id 结尾 下一个节点id 返回 /*HH1！ 1*/normalize_winner： //按因素推进，使获胜者 //活动1和最小的活动0 gosub查找最小最大ct //升压 对于nodes_id=first_node（current_grid）到last_node（current _ grid） 设置节点输出节点id，（节点输出（节点id）-最小act）/（最大act最小act 下一个节点id 返回 /*HH1！ 1*/横向禁止： //通过活动节点的平均活动抑制所有 //求平均值 float av_act=0 float node_number=0 对于nodes_id=first_node（current_grid）到last_node（current_grid） 如果node_output（nodes_id）>0.01 av_act=av_act+节点输出（节点id） 节点编号=节点编号+1 结尾 下一个节点id av_act=0.8*av_act/节点编号 //当所有活动节点具有相同的输出时，可以防止出现问题 如果node_number=1 av_act=0 结尾 //减去平均值 对于nodes_id=first_node（current_grid）到last_node（current _ grid） 如果node_output（nodes_id）>av_act 集合节点输出节点id，节点输出（节点id）-平均act 其他的 集合节点输出节点id，0 结尾 下一个节点id 返回 /*SH1！ 1*/ /*#CP=29256:XX=99*/