PSALMS

janeconway Projects

Psalms is the Autonomous Wheelchair constructed by Guido Bugmann for Donald Rodney’s “Nine Night in Eldorado” at the South London Gallery, 1997.

“…Our fear of automata is again harnessed in Psalms, as the empty wheelchair courses through its various trajectories on a sad and lonely journey of life, a journey to nowhere. Its movements repeat like an ever recurring memory, a memory of another life and another journey, that of Donald Rodney’s father…”
(Exhibition brochure, Jane Bilton.)

Psalms. On show at Plymouth Council House for the Atlantic Project 28 September – 21 October 2018.

pslams1
psalms8pslams10

Psalms in action at the opening of “Nine Night in Eldorado” at the South London Gallery 1997.

Short article published in Mediaspace: http://www.mike-phillips.net/blog/wp-content/uploads/2014/01/mediaspace4.pdf
mediaspace4_page_21
mediaspace4_page_23

The work has also been exhibited at:

‘The political significance of Rodney’s work should not be underestimated, nor his legacy which continues to inspire younger artists.’ (Keith Piper, Co-curator).

The web page by Dr Guido Bugmann tracks the various installations and technical details for the commision.
http://www.tech.plym.ac.uk/soc/staff/guidbugm/wheelchair/wheelc.htm
(pdf version here).
20160922_160322
Reboot 2016.
The wheelchair uses 8 sonar sensors, shaft-encoders, a video camera and a rate gyroscope to determine its position. A neural network using normalised RBF nodes encodes the sequence of 25 semi-circular sequences of positions forming the trajectory. The control system comprises a laptop PC 586 running a control program written in CORTEX-PRO, and linked to a Rug Warrior board built around the 68000 microcontroller.
Original (with some tweaks) Psalms Code (courtesy of Guido Bugmann):

/*HS!!1*/// program WCSHOW8.CTX
 // version 14 Sept 97, 15 March 98, Feb 2003, Oct 2008 (INIVA gallery London)
 // version Nov 2009
 // version june - july 2012 Wellcome exhibition
 // wheelchair project 97
 // modif of wcshow2.ctx by adding correct
 // trajectories encoding from wcshow3.ctx
 //from wcshow6.ctx
/*SH1!1*///variables // new. Including room dimensions
 /*HS!!2*///----------comm variables--------------
 int Eureka =104, scope_data = 110, gyro_data = 105, gyro_reset = 125
 int reset_shaft_xyphi = 120
 int battery_data = 106
 int 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 message_byte, b1, b2, b3
 int serial_count, serial_count_limit = 500
 // a smaller limit may be better
 float float_data
 int left_speed = 1, right_speed = 1
 float right_speed_desired = 0
 float left_speed_desired = 0
/*HH2!2*///----------position variables--------------
 float PI = 4*arctan(1)
float phi2, angle1, angle2, delta_phi, angle_difference
float x_wc=0, y_wc=0, phi_wc=0
 float x_wc_shift=0, y_wc_shift=0, phi_wc_shift=0
 float old_x_wc=0, old_y_wc=0, old_phi_wc=0
 float d_x_wc_dt, d_y_wc_dt, d_phi_wc_dt
 float x_wc_predict, y_wc_predict, phi_wc_predict
 float previous_phi_wc, previous_x_wc, previous_y_wc
float x_shaft, y_shaft, phi_shaft
 float w_x_shaft, w_y_shaft, w_phi_shaft, w0_shaft
 float d_x_shaft_dt, d_y_shaft_dt, d_phi_shaft_dt
 float old_x_shaft, old_y_shaft, old_phi_shaft
float phi_gyro, w0_gyro, w_phi_gyro, d_phi_gyro_dt
float phi_camera, w_phi_camera, d_phi_camera_dt
 float phi_cam1, phi_cam2
float x_sonar, y_sonar, phi_sonar
 float w0_sonar, w_x_sonar, w_y_sonar, w_phi_sonar
 float d_x_sonar_dt, d_y_sonar_dt, d_phi_sonar_dt
float last_time_shaft, last_time_motor
/*HH2!2*/// ---------wheelchair---------
 float delta_r = 56.5
 /*HH2!2*///----------R O O M Dimensions at Wellcome trust ------------
float max_room_xcm = 1000 //694
 float max_room_ycm = 1100 // 1005
 /*HH2!2*///----------trajectory---------------//new
 int section_list[32]
 /*HH2!2*///----------neural networks --------------- // new
float theta, theta_next, r
int steps_nb_max = 16
 int k, current_grid
 int section_size[40]
 /float section_boost= 150
 / old section based
 /float section_boost= 50
 / new node based for trajectory
float section_boost= 90 // test using 1. Should be 90
 / new node based for field drawing
/float section_boost= 0.3
 / very new node + sigma based for trajectory
 float forced_next_state
/*HH2!2*///----------speed variables----------// new
 float section_speed[200]
 int current_section, temp_left_speed, temp_right_speed
 /*HH2!2*///-----------sonar --------------
 int keyboard, color, sonar, alarm_persistence = 0
 float time_since_last_alarm = 0, time_of_last_alarm = 0
int min_range_check = 20, max_sonar_array = 1500
 int echo[max_sonar_array]
 float found_flag, xs, ys
 float sonar_dir[8+1], sonar_range[8+1], old_sonar_range[8+1], sonar_x[8+1], sonar_y[8+1]
 int front_obstacle, left_obstacle, right_obstacle
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
 sonar_x[1] = 0
 sonar_y[1]= -10
 sonar_x[2] = -26
 sonar_y[2]= 7
 sonar_x[3] = -26
 sonar_y[3]= 37
 sonar_x[4] = -26
 sonar_y[4]= 51
 sonar_x[5] = 0
 sonar_y[5]= 51
 sonar_x[6] = 26
 sonar_y[6]= 51
 sonar_x[7] = 26
 sonar_y[7]= 37
 sonar_x[8] = 26
 sonar_y[8]= 7
float sonar_d1, sonar_d2, sonar_d3, sonar_d4
 /*HH2!2*/// -----pcmcia card--------
//int porta = 256, portb = 257
 int porta = 8192, portb = 8193
 //int porta = 992, portb = 993
 //int porta = 640, portb = 641
 / (hex 100 as defined in the dc24cl.com call in autoexec.bat)
 / (hex 3e0 = 992 dec as defined in the dc24cl.com call in autoexec.bat)
 // hex 280 = dec 640
 /*HH2!2*///---------video------------
 float contrast_gain = 5
 float av_act =0, max_act = 0, w0_camera
 float xsize = 0.97
 float ysize = 0.97
 float xcentre = 0.5
 float ycentre = 0.5
 int nodes_id, max_search = 50
 int Houghinit = 0
/*HH2!1*///--- set serial port mode----
 //ctx_system "mode com1:1200,n,8,1"
// ctx_system "mode com2:9600,n,8,1"
 // int port = 2
// ctx_system "mode com1:9600,n,8,1"
 ctx_system "mode com1:9600,n,8,1,p"
 int port = 1
 //pak
/*HH1!1*///====defines buttons =========
 defpan progpan
 button "return", "poppan"
 butgap
 //button "Send", "gosub Send"
 button "STOP", "gosub stop_motors"
 button "Motors_on", "gosub send_motor_command"
 butgap
 //button "Scope", "gosub get_scope_data"
 button "Sonar", "gosub sonar_scan8"
 /butgap
 /button "Video", "gosub get_camera_data"
 /button "QC init", "gosub Houghinitqc"
 butgap
 button "Reset_Gyro", "gosub reset_gyro"
 button "Gyro", "gosub get_gyro_data"
 butgap
 button "Reset Shaft", "gosub reset_shaft_position_to_zero"
 button "Shaft_pos", "gosub get_shaft_position_data"
 butgap
 button "Track Pos.", "gosub procedure_track_position"
 pushpan progpan
/*HH1!1*/// graphs and windows
float max_steps = 255
graph_default_gridtype 0
 graph_default_axislabel "Steps", "Data"
 graph_default_range 0, 0, max_steps, 255
 makegraph Data
graph_default_axislabel "Range", "Sensor"
 graph_default_range 0, 0, float(max_sonar_array), 10
 makegraph Sonar
/win_default_range 0,0,1,1
 /makewin Image
win_default_range -50,-50,50+max_room_xcm,50+max_room_ycm
 makewin Room
//win_plot 0, 0, RED
 //win_line 1000, 1100, YELLOW
 /*HH121*/// Build network and define trajectory sections to be used
 /gosub build
 / needed only if image is used
/gosub build_a_circle
 /*HS!!2*/// define section list // new - need to reflect in "build trajectory"
 for i=1 to 31
 section_list[i]=-99
 next i
 // actually used sections:
 section_list[1]=1
 section_list[2]=2
 section_list[3]=3
 section_list[4]=4
 section_list[5]=13
 section_list[6]=14
 section_list[7]=15
 section_list[8]=16
 // needs to be in ascending order for now.
gosub build_trajectories
 /*HH1!1*/int ON_BRICKS = 0 //if on bricks = 1 -> sonar-based obst. avoid disabled
 int SIMULATION = 0
 // ---> clear and run the program if the values above are changed
 /*SH1!1*/end
 /*SS!!1*/
 /*HS!!1*/procedure_track_position: // Initial position to set here
/*HS!!2*/restart_procedure:
w0_shaft = 1
 w0_gyro=1
 w0_camera = 0
 w0_sonar =1
alarm_persistence=0
float l_arrow = max_room_xcm/15
/-----------------------------------
 /*HH2!2*/// initialise the robot position:
 /-----------------------------------
 // here we decide where he starts and where he looks.
 // in the final program, he will find out by himself.
/gosub procedure_self_localisation
// x_wc = max_room_xcm/2
 x_wc = 700 // For Wellcome Trust
 y_wc = 0
 phi_wc = PI/2 //towards middle of room = north
 // phi_wc = 0 // towards right.
 // phi_wc = PI/2 - 0.35 //North, slightly to the East.
/ to start the sequence:
 //set_grid_outputs Sequence_State, 1*section_boost
 set_grid_outputs Sequence_State, 5*section_boost
old_x_wc = x_wc
 old_y_wc = y_wc
 old_phi_wc = phi_wc
old_x_shaft = x_wc
 old_y_shaft = y_wc
 old_phi_shaft = phi_wc
gosub reset_shaft_position
 gosub reset_gyro
old_x_shaft=x_wc
 old_y_shaft=y_wc
 old_phi_shaft=phi_wc
if SIMULATION=1
 float next_x = x_wc
 float next_y = y_wc
 float next_phi = phi_wc
 endif
 /-----------------------------------
 /*HH2!2*/// initialise the robot's speed and speed per section // new
 /-----------------------------------
 float base_speed = 20 // 15 //13 //what is the max?
 // 20 is right for Wellcome
 left_speed = 1
 right_speed = 1
 gosub send_motor_command
 // to preload the capacitor and prevent
 // a start backwards
 /test
 /base_speed = 10
 left_speed_desired = 0
 right_speed_desired = 0
 /----------------------------------------
 // start large circles-------------
 for k = 0 to 12
 section_speed[k]=0.8*base_speed //was 0.7
 next k
 for k = 13 to 24
 section_speed[k]=0.8*base_speed
 next k
 for k = 25 to 48
 section_speed[k]=base_speed
 next k
 // start inwards spiral------------
 for k = 49 to 60
 section_speed[k]=0.9*base_speed
 next k
 for k = 61 to 70
 section_speed[k]=0.8*base_speed
 next k
 for k = 71 to 78
 section_speed[k]=0.6*base_speed
 next k
 for k = 79 to 83
 section_speed[k]=0.5*base_speed
 next k
 for k = 84 to 87
 section_speed[k]=0.4*base_speed
 next k
 // start figures of eight------------
 for k = 88 to 94
 section_speed[k]=0.4*base_speed
 next k
 for k = 95 to 156
 section_speed[k]=0.5*base_speed
 next k
 for k = 157 to 162
 section_speed[k]=0.4*base_speed
 next k
 // start outward spiral-------------
 for k = 163 to 166
 section_speed[k]=0.4*base_speed
 next k
 for k = 167 to 175
 section_speed[k]=0.5*base_speed
 next k
 for k = 176 to 185
 section_speed[k]=0.7*base_speed
 next k
 for k = 186 to 199
 section_speed[k]=0.8*base_speed
 next k
 /---------------------------------------------------------------
 /*HH2!2*/// Main control Loop (get position data, plot, compute controls)
 /---------------------------------------------------------------
win_active Room
 win_clear
 print "PRESS ANY KEY TO STOP (but not ESC)."
 / so as not to stop in the middle of a communication process
win_move 0,0
 win_line max_room_xcm,0, 4
 win_line max_room_xcm, max_room_ycm, 4
 win_line 0, max_room_ycm, 4
 win_line 0, 0, 4
init_timer
 last_time_shaft = 0
 last_time_motor = 0
/*HH2!2*/while !kbhit()
keyboard = port_in(96)
/ /my_sound 5000,30
 if elapsed_time(1) > 100000
 init_timer
 last_time_shaft = 0
 last_time_motor = 0
 endif
/*HH2!2*/ //gosub get_shaft_position_data
 if SIMULATION=1
 x_shaft= x_wc+ 0.3*(next_x-x_wc)
 y_shaft = y_wc+0.3*(next_y-y_wc)
 phi_shaft =next_phi// +0.1*(next_phi-phi_wc)
 else
 gosub get_shaft_position_data
 endif
w_x_shaft = 0.5
 w_y_shaft = 0.5
 w_phi_shaft = 0.25
if y_shaft=0 & x_shaft = 0
 goto restart_procedure
 endif
 /win_line x_shaft, y_shaft, WHITE
 /*HH2!2*/ gosub predict_current_shaft_position
/ gosub get_gyro_data
 / w_phi_gyro = 0.25
 / print " Phi G Y R O = ", phi_gyro*180/PI
/*HH2!2*/ // gosub get_sonar_data
 if SIMULATION!=1
 gosub get_sonar_data
 endif
 /*HH2!2*/ gosub check_sonar_validity
 phi_sonar = phi_sonar + 0.45*d_phi_shaft_dt
 / the 0.45 sets the value to that at the time when the RugWarrior
 / is updated (the same 0.45 is used to bring back to the present the
 / angles in "check_sonar_validity")
 /*HH2!2*/// if necessary (robot stuck in a corner), relocalise
 //if alarm_persistence > 6
 // gosub relocalise
 //endif
/ gosub get_camera_data
 / w_phi_camera = 0
 x_wc = (w_x_shaft*x_shaft+w_x_sonar*x_sonar)/(w_x_shaft+w_x_sonar)
 y_wc = (w_y_shaft*y_shaft+w_y_sonar*y_sonar)/(w_y_shaft+w_y_sonar)
//print "====++++++ y_wc = ", y_wc, " y_shaft = ", y_shaft
 /*SH2!2*///print "====++++++ x_wc = ", x_wc, " x_shaft = ", x_shaft
 /*HS!!2*/// calculate weight-averaged position values
angle1=phi_shaft
 angle2=phi_sonar
 gosub angle_diff
/ if abs(angle_difference) > 0.3*PI
 / w_phi_sonar=0
 / endif
 phi_wc = (phi_shaft*(w_phi_shaft+w_phi_sonar) + w_phi_sonar * angle_difference)/(w_phi_shaft+w_phi_sonar)
/----wc positions are estimated at time present---
 / later this will be a weighted average
 / also including time-delays and rates of changes d_bla_dt
 /*HH2!2*/// reset values in RugWarrior board
 if w_phi_sonar > 0 | w_x_sonar > 0 | w_y_sonar > 0
 angle1=0
 angle2=phi_wc
 gosub angle_diff
 phi_wc = angle_difference
 gosub reset_shaft_position
 /gosub reset_gyro
 / /my_sound 1000,30
 endif
 /*HH2!2*/ //----------position and sonar plot-----------
 win_fillellipse x_wc-0.1*l_arrow, y_wc-0.1*l_arrow, x_wc+0.1*l_arrow,y_wc+0.1*l_arrow, LBLUE
 win_move x_wc, y_wc
 win_line x_wc+l_arrow*cos(phi_wc), y_wc+l_arrow*sin(phi_wc), BLUE
//----------sonar plot---------------
 gosub sonar_plot
/------------------------------
 /*SH2!2*/// calculate a new motor command - using neural network
 /------------------------------
 / (simplified here)
 // left_speed = 15
 // right_speed = 15
 / gosub feedback_straight_line
gosub predict_position_of_control_action
 / gosub update_network
 / for the circle
 gosub update_network2
 gosub calculate_new_motor_command
 /*HS!!2*/// deal with obstacles (stop or avoid) or execute the motor command
temp_left_speed = left_speed
 temp_right_speed = right_speed
gosub sonar_safety // disabled for trial on bricks
/*HH232*/// -> avoidance manoeuvres (not if ON BRICKS)
 if ON_BRICKS = 1
 goto no_avoidance
 endif
 if alarm_counter <= 2 // >= 2
 int motion_duration = 4500 // 3200 // 2800 // 2400 for short avoidance moves
 /*SS!!3*/ // 1. case of obstacle in front -> turn in the previous direction
if front_obstacle =1 & left_obstacle = 0 & right_obstacle=0
if delta_phi < 0
 left_speed = 18 //10
 right_speed = 1
 else
 left_speed = 1
 right_speed = 18 //10
 endif
 gosub send_motor_command
 ms_delay motion_duration
print "STOP AFTER ESCAPE MANOEUVRE"
 left_speed = 0
 right_speed = 0
 gosub send_motor_command
 endif
/*HS!!3*/ // 2. left or right obstacle -> turn to escape // new 23.2.03
if left_obstacle = 1 & right_obstacle = 0
 left_speed = 18 //10
 right_speed = 1
 gosub send_motor_command
 ms_delay motion_duration //1500
 print "STOP AFTER ESCAPE MANOEUVRE"
 left_speed = 0
 right_speed = 0
 gosub send_motor_command
 endif
if right_obstacle = 1 & left_obstacle = 0
 left_speed = 1
 right_speed = 18 //10
 gosub send_motor_command
 ms_delay motion_duration //1500
 print "STOP AFTER ESCAPE MANOEUVRE"
 left_speed = 0
 right_speed = 0
 gosub send_motor_command
 endif
/*SH3!3*/ // 3. Obstacles on both sides -> do not move
 if right_obstacle = 1 & left_obstacle = 1
 left_speed = 1
 right_speed = 1
 gosub send_motor_command
 print "SQUEEZED ON BOTH SIDES"
 my_sound 3000, 150 // short high-pitched beep
 ms_delay 1500
 endif
/*HS!!3*/ // 4. If too close in front -> stop and beep (commented out)
/ if sonar_range[4]<0 & sonar_range[5]<0 & sonar_range[6]<0
 / my_sound 6000,100
 / endif
/*HH3!3*/ // 4. stop after the escape manoeuvre
//print "STOP AFTER ESCAPE MANOEUVRE" - now done inside each if above.
 //left_speed = 0
 //right_speed = 0
 //gosub send_motor_command
else
 /*HH2!2*/// or proceed with previous command
 left_speed = temp_left_speed
 right_speed = temp_right_speed
 // gosub send_motor_command //gb 23.10.08
endif
/*HH2!2*/no_avoidance:
gosub send_motor_command //gb 23.10.08
old_x_wc = x_wc
 old_y_wc = y_wc
 old_phi_wc = phi_wc
 // used also in predict and sonar plot...
 /pak
 /*SH2!2*/endwhile
//pak
 //int bla=getch()
gosub stop_motors
 /----------------------------------------------
 return
 /*SH1!1*/
 /*HS!!1*/predict_current_shaft_position: //delays adjustable
if SIMULATION=1
 return
 endif
float delay1 = 1.20
 float time_delay_shaft = elapsed_time(1)-last_time_shaft
 // 0.94= times in seconds to send back a value in phase with RugW.
 // previous 0.6, 0.5 was ok, 0.42, 0.695
 //print "---------- ", time_delay_shaft
 //NOTE: I assume that delay1 + delay2 (next subroutine) = total time delay
 // about 2.5 sec at Wellcome
float local_x, local_y, local_phi
d_x_shaft_dt = delay1*(x_shaft - old_x_shaft)/time_delay_shaft
 d_y_shaft_dt = delay1*(y_shaft - old_y_shaft)/time_delay_shaft
angle1 = old_phi_shaft
 angle2 = phi_shaft
 gosub angle_diff
d_phi_shaft_dt = delay1*(angle_difference)/time_delay_shaft
/d_phi_shaft_dt = delay1*(phi_shaft - old_phi_shaft)/time_delay_shaft
float delta_s = sqrt(d_x_shaft_dt * d_x_shaft_dt + d_y_shaft_dt * d_y_shaft_dt)
 local_x = x_shaft + delta_s*cos(phi_shaft+0.5*d_phi_shaft_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_shaft
 old_y_shaft=y_shaft
 old_phi_shaft=phi_shaft
x_shaft = local_x
 y_shaft = local_y
 phi_shaft = local_phi
/win_move local_x, local_y
last_time_shaft = elapsed_time(1)
return
 /*HH1!1*/predict_position_of_control_action: //delays adjustable
if SIMULATION!=1
 float delay2 = 1.3// 1.75
 float time_delay_motor = elapsed_time(1)-last_time_motor
 // previous 1.5
 //print "----------------", time_delay_motor
 float local_x, local_y, local_phi
d_x_wc_dt = delay2*(x_wc - old_x_wc)/time_delay_motor
 d_y_wc_dt = delay2*(y_wc - old_y_wc)/time_delay_motor
 d_phi_wc_dt = delay2*(phi_wc - old_phi_wc)/time_delay_motor
/old_x_wc=x_wc
 /old_y_wc=y_wc
 /old_phi_wc=phi_wc
float delta_s = sqrt(d_x_wc_dt * d_x_wc_dt + d_y_wc_dt * d_y_wc_dt)
 local_x = x_wc + delta_s*cos(phi_wc+0.5*d_phi_wc_dt)
 local_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 = local_x
 y_wc_predict = local_y
 phi_wc_predict = local_phi
/win_move local_x, local_y
/printf "Prediction %f, %f, %f\n", x_wc_predict, y_wc_predict, phi_wc_predict*180/PI
last_time_motor = elapsed_time(1)
 else // in simulation mode
 x_wc_predict = x_wc
 y_wc_predict = y_wc
 phi_wc_predict = phi_wc
endif
 return
 /*HH121*/sonar_safety:
if ON_BRICKS = 1
 return
 endif
 // This routine stops the wheelchair until the obstacle has moved.
 // if after 1200 ms the obstacle is still there, the control is
 // returned to the main loop which will execute an escape movement.
int shaft_encoder_hold = 0
 int alarm_counter=0
/*HS!!2*/detect_an_obstacle:
 front_obstacle=0
 left_obstacle=0
 right_obstacle=0
//for sonar = 4 to 6
 // gosub fast_get_sonar_range
 // acquire fresh data in front
 // if sonar_range[sonar]<150 & sonar_range[sonar] > 0
 // front_obstacle = 1
 //print "FRONT = ", sonar, " range = ", sonar_range[sonar]
 //pak
 /goto sonar_stop
 // endif
 //next sonar
 // idea, not do all front sonars one after the other, to avoid stray echos?
 sonar =4
 gosub fast_get_sonar_range
 // acquire fresh data in front
 if sonar_range[sonar]<150 & sonar_range[sonar] > 0
 front_obstacle = 1
 endif
sonar = 2
 gosub fast_get_sonar_range
 sonar = 3
 gosub fast_get_sonar_range
 sonar =5
 gosub fast_get_sonar_range
 // acquire fresh data in front
 if sonar_range[sonar]<150 & sonar_range[sonar] > 0
 front_obstacle = 1
 endif
sonar = 7
 gosub fast_get_sonar_range
 sonar = 8
 gosub fast_get_sonar_range
 sonar =6
 gosub fast_get_sonar_range
 // acquire fresh data in front
 if sonar_range[sonar]<150 & sonar_range[sonar] > 0
 front_obstacle = 1
 endif
if sonar_range[2]<80 & sonar_range[2] > 0
 left_obstacle=1
 //print " sonar 2: ", sonar_range[2], " sonar 3: ", sonar_range[3]
 /goto sonar_stop
 endif
 if sonar_range[8]<80 & sonar_range[8] > 0
 right_obstacle=1
 /goto sonar_stop
 endif
 if sonar_range[3]<60 & sonar_range[3] > 0
 left_obstacle=1
 /goto sonar_stop
 endif
 if sonar_range[7]<60 & sonar_range[7] > 0
 right_obstacle=1
 /goto sonar_stop
 endif
if front_obstacle = 1 | left_obstacle=1 | right_obstacle=1
 goto sonar_stop
 endif
if shaft_encoder_hold = 1
 gosub reset_shaft_position
 //gosub reset_gyro
 endif
 //print "NO OBSTACLES : SAFE"
 return
 /--------------------------------
 /*HH2!2*/sonar_stop:
 alarm_counter +=1
left_speed = 0
 right_speed = 0
 gosub send_motor_command
 //my_sound 9000,100
 print "SONAR STOP ", "obstacles L F R=", left_obstacle, " ", front_obstacle, " ", right_obstacle
 ms_delay 1200
if shaft_encoder_hold = 0
 old_x_wc = x_wc
 old_y_wc = y_wc
 old_phi_wc = phi_wc
 shaft_encoder_hold = 1
 endif
if alarm_counter >= 2
if shaft_encoder_hold = 1
 gosub reset_shaft_position
 gosub reset_gyro
 endif
time_since_last_alarm = elapsed_time(1)-time_of_last_alarm
if time_since_last_alarm < 20
 alarm_persistence +=1
 else
 alarm_persistence =0
 endif
time_of_last_alarm = elapsed_time(1)
return
 endif
goto detect_an_obstacle
return
 /*HH121*/check_sonar_validity:
float tested_phi = phi_shaft - 0.45*d_phi_shaft_dt
 / a that point in the program, we still use the previous value of phi_wc.
 / therefore, the tested value
 float angle_tolerance = 12*PI/180
 // this tolerance should be dependent on the angular velocity.
 // when it is fast, there are less measurement points and
 // the tolerance should increase.
 float xy_tolerance = 40
w_x_sonar = 0
 w_y_sonar = 0
 w_phi_sonar = 0
x_sonar = 0
 y_sonar = 0
 phi_sonar = 0
found_flag =0
angle1=0
 angle2=tested_phi
 gosub angle_diff
/*SS!!2*/if abs(angle_difference) < angle_tolerance
 // case phi=0; look at the curtain: sonar_d2
 if abs(sonar_d2 - max_room_ycm + y_wc) < xy_tolerance & sonar_range[3]>0
 y_sonar += max_room_ycm - sonar_d2
 phi_sonar = 0
 w_y_sonar +=0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 2000,100
 endif
if abs(sonar_d1 - x_wc) < xy_tolerance & sonar_range[1]>0
x_sonar += sonar_d1
 phi_sonar = 0
 w_x_sonar +=0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 2000,100
 endif
if abs(sonar_d3 - max_room_xcm + x_wc) < xy_tolerance & sonar_range[5]>0
x_sonar += max_room_xcm - sonar_d3
 phi_sonar = 0
 w_x_sonar +=0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 2000,100
 endif
endif
/*HS!!2*/if abs(angle_difference-0.5*PI) < angle_tolerance
 // case phi=PI/2; look at the nearest side wall or both d2 left, d4 right
if abs(sonar_d4 - max_room_xcm + x_wc) < xy_tolerance & sonar_range[7]>0
 x_sonar += max_room_xcm - sonar_d4
 phi_sonar = 0.5*PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 3000,100
 endif
if abs(sonar_d2 - x_wc) < xy_tolerance & sonar_range[3]>0
 x_sonar += sonar_d2
 phi_sonar = 0.5*PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 3000,100
 endif
if abs(sonar_d1 - y_wc) < xy_tolerance & sonar_range[1]>0
 y_sonar += sonar_d1
 phi_sonar = 0.5*PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 3000,100
 endif
if abs(sonar_d3 - max_room_ycm + y_wc) < xy_tolerance & sonar_range[5]>0
 y_sonar += (max_room_ycm - sonar_d3)
 phi_sonar = 0.5*PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 3000,100
 endif
endif
/*HH2!2*/if abs(abs(angle_difference)-PI) < angle_tolerance
 // case phi=PI; look at the curtain d4
 if abs(sonar_d4 - max_room_ycm + y_wc) < xy_tolerance & sonar_range[7]>0
 y_sonar += max_room_ycm - sonar_d4
 phi_sonar = PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 5000,100
 endif
if abs(sonar_d2 - y_wc) < xy_tolerance & sonar_range[3]>0
 y_sonar += sonar_d2
 phi_sonar = PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 5000,100
 endif
if abs(sonar_d3 - x_wc) < xy_tolerance & sonar_range[5]>0
 x_sonar += sonar_d3
 phi_sonar = PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 5000,100
 endif
if abs(sonar_d1 - max_room_xcm + x_wc) < xy_tolerance & sonar_range[1]>0
 x_sonar += max_room_xcm - sonar_d1
 phi_sonar = PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 5000,100
 endif
endif
 /*HH2!2*/if abs(angle_difference+0.5*PI) < angle_tolerance
// case phi = -PI/2; look at side walls d4 (small x) and d2 large x
 if abs(sonar_d4 - x_wc) < xy_tolerance & sonar_range[7]>0
 x_sonar += sonar_d4
 phi_sonar = -0.5*PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 4000,100
 /print " Phi_wc= ", phi_wc, " Phi_sonar = ", phi_sonar
 /pak
 endif
 if abs(sonar_d2 - max_room_xcm + x_wc) < xy_tolerance & sonar_range[3]>0
 x_sonar += max_room_xcm - sonar_d2
 phi_sonar = -0.5*PI
 w_x_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 4000,100
 endif
if abs(sonar_d1 - max_room_ycm + y_wc) < xy_tolerance & sonar_range[1]>0
 y_sonar += max_room_ycm - sonar_d1
 phi_sonar = -0.5*PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 4000,100
 endif
if abs(sonar_d3 - y_wc) < xy_tolerance & sonar_range[5]>0
 y_sonar += sonar_d3
 phi_sonar = -0.5*PI
 w_y_sonar += 0.25
 w_phi_sonar += 0.25
 found_flag +=0.25
 /my_sound 4000,100
 endif
endif
/*HH2!2*/// average values
 if w_x_sonar > 0
 x_sonar = 0.25*x_sonar/w_x_sonar
 endif
if w_y_sonar > 0
 y_sonar = 0.25*y_sonar/w_y_sonar
 endif
w_x_sonar = 0.8*w_x_sonar
 w_y_sonar = 0.8*w_y_sonar
 w_phi_sonar = 0.8*w_phi_sonar
 /print "ywc = ", y_wc, " ysonar= ",y_sonar
return
 /*HH121*/relocalise:
print "RELOCALISE"
 alarm_persistence=0
/*HS!!2*/if sonar_d2 < 120 & sonar_range[3]>0
 // left turn desired
 angle2 = phi_wc
 angle1= 0
 gosub angle_diff
if abs(angle_difference) < PI/5
 y_shaft = max_room_ycm - 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
if abs(angle_difference)-0.5*PI < PI/5
 x_shaft = 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference)+0.5*PI < PI/5
 x_shaft = max_room_xcm - 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference)-PI < PI/5
 y_shaft = 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
return
endif
 /*HH2!2*/if sonar_d4 < 120 & sonar_range[7]>0
 // right turn desired
 angle2 = phi_wc
 angle1= 0
 gosub angle_diff
if abs(angle_difference)-PI < PI/5
 y_shaft = max_room_ycm - 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
if abs(angle_difference)-0.5*PI < PI/5
 x_shaft = max_room_xcm - 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference)+0.5*PI < PI/5
 x_shaft = 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference) < PI/5
 y_shaft = 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
return
endif
/*HH2!2*/if sonar_d3 < 120 & sonar_range[5]>0
 // heading towards a wall
 angle2 = phi_wc
 angle1= 0
 gosub angle_diff
if abs(angle_difference)-0.5*PI < PI/5
 y_shaft = max_room_ycm - 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
if abs(angle_difference) < PI/5
 x_shaft = max_room_xcm - 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference)-PI < PI/5
 x_shaft = 100
 x_sonar = x_shaft
 w_x_sonar = 1
 w_x_shaft = 1
 return
 endif
if abs(angle_difference)+0.5*PI < PI/5
 y_shaft = 100
 y_sonar = y_shaft
 w_y_sonar = 1
 w_y_shaft = 1
 return
 endif
return
endif
return
 /*SH1!1*/
 /*SS!!1*/calculate_new_motor_command:
float next_x = node_output(first_node(Next_Position))
 float next_y = node_output(first_node(Next_Position)+1)
 float next_phi = node_output(first_node(Next_Position)+2)
win_fillellipse next_x-0.1*l_arrow, next_y-0.1*l_arrow, next_x+0.1*l_arrow, next_y+0.1*l_arrow, RED
 win_move next_x, next_y
 win_line next_x+l_arrow*cos(next_phi), next_y+l_arrow*sin(next_phi), YELLOW
/*HS!!2*/// next point as target. phi2 = angle to target
float dx = next_x - x_wc
 float dy = next_y - y_wc
 float dist = sqrt(dx*dx+dy*dy)
phi2 = 999
if dx=0 & dy >0
 phi2 = PI/2
 endif
 if dx=0 & dy <0
 phi2 = -PI/2
 endif
phi2 = arctan(dy/dx)
if dy<0 & dx<0
 phi2 = phi2 + PI
 endif
 if dy>=0 & dx<0
 phi2 = phi2 + PI
 endif
 if dy<0 & dx>0
 phi2 =phi2 + 2*PI
 // to avoid all negative angles
 endif
 // phi2 is where the robot should point to reach the target
/*HH2!2*/// ---- difference between current robot heading phi_wc and desired one phi2
/ try use the predicted angle when the action starts:
 / not a good idea, there are big jumps in prediction
 / when sonar data are recorded
 //
 //float phip = phi_wc_predict
 // instead of phi_wc
float sind=(cos(phi2)*sin(phi_wc)-sin(phi2)*cos(phi_wc))
 float cosd = (cos(phi2)*cos(phi_wc)+sin(phi2)*sin(phi_wc))
 float local_tan = sind/cosd
 float delta_phi = arctan(local_tan)
if sind >=0 & cosd <0
 delta_phi += PI
 endif
 if sind < 0 & cosd < 0
 delta_phi -= PI
 endif
 delta_phi =-delta_phi
/*SH2!2*//----speed control ------- new - Wellcome trust
base_speed = section_speed[current_section]
 float dist0=1 //m
 float w1 = 1/(1+exp(-(0.01*dist-dist0))) //dist in m, for control by angle to target
 float w2 = 1/(1+exp(+(0.01*dist-dist0))) //dist in m, for control by heading diff (delta_phi)
 // distance effect
 //print "w1= ", w1, " w2= ", w2
 angle1 = phi_wc
 angle2 = next_phi
 gosub angle_diff
 //float exp2 =exp(-0.5*(angle_difference)*(angle_difference))
 // direction effect
float speed = 0.5*base_speed*(1+exp(-abs(delta_phi)))
 // all about adjusting the speed to angle differences
 if phi2 = 999
 speed = 0
 endif
 //print "base_speed= ", base_speed, " speed = ", speed
//float exp3 =exp(-0.5*delta_phi*delta_phi)
 float sp_gain = 1.1 //1 alllowed too much excursion // 0.82+0.9*(exp3)
 float vl1 = 2*speed/(1+exp( + sp_gain*delta_phi) )
 float vr1 = 2*speed/(1+exp( - sp_gain*delta_phi) )
 float vl2 = 2*speed/(1+exp( + sp_gain*angle_difference) )
 float vr2 = 2*speed/(1+exp( - sp_gain*angle_difference) )
 print "vl1= ", vl1, " vr1= ",vr1
 print "vl2= ", vl2, " vr2= ",vr2
 left_speed_desired = fmax(1, w1*vl1 + w2*vl2)
 right_speed_desired = fmax(1, w1*vr1 + w2*vr2)
/float sp_gain = 0.5+0.3*speed/15
 /float sp_mom = 0.2
 /left_speed_desired = sp_mom*left_speed_desired + (1-sp_mom)*speed/(1+exp( + sp_gain*(delta_phi)) )
 /right_speed_desired = sp_mom*right_speed_desired + (1-sp_mom)*speed/(1+exp( - sp_gain*(delta_phi)) )
 /float lambda = 0.4
 //old good values
 //----------
 ///float speed = 0.7*base_speed + 0.3*base_speed*(exp1*exp2*exp3)
 ///float sp_gain = 0.82+0.9*(exp3)
 float sp_mom = 0.2 // 0.22+(0.8/speed)
 left_speed_desired = (1-sp_mom)*left_speed_desired + sp_mom*float(left_speed)
 right_speed_desired = (1-sp_mom)*right_speed_desired + sp_mom*float(right_speed)
 //float lambda = 0.54*sp_gain
 // seemed promissing during the night
 //float lambda = 0.5 //gbtest 090712
 //-----------
 left_speed = int(left_speed_desired)
 right_speed = int(right_speed_desired)
 print "Ldes = ", left_speed_desired , " Rdes. = ", right_speed_desired, " phi_wc = ", 180*phi_wc/PI
 ///left_speed = int(left_speed_desired + lambda*(left_speed_desired -float(left_speed)))
 ///right_speed = int(right_speed_desired + lambda*(right_speed_desired -float(right_speed)))
/*SS!!2*/return
 /*HS!!1*/send_motor_command: //motor bias in here.
//int direction_byte = 0
 //if right_speed_desired >= 0
 // direction_byte +=1
 //endif
 //if left_speed_desired >= 0
 // direction_byte +=2
 //endif
 // TO include when backwards motion is
 // considered in the RugWar board
message_byte = 107
 gosub handshake_send_byte
// speeds must not exceed +-100
message_byte = int(0.80*float(left_speed)) + 128
 //0.8 foro Wellcome, was 0.85
 //message_byte = int(1.1*float(left_speed)) + 128
 // the coef 1.1 compensates for an asymetry between left and right
 // in the Penny and Gilles system.
 // seems to be thr other way now (Nov 2009) right wheel is slugish.
 gosub handshake_send_byte
//message_byte = right_speed + 128
 message_byte = int(1.15*float(right_speed)) + 128
gosub handshake_send_byte
//message_byte = direction_byte
 //gosub handshake_send_byte
 // TO include when backwards motion is
 // considered in the RugWar board
 print "sent L=",left_speed, " R=", right_speed
 return
 /*HH1!1*/stop_motors:
message_byte = 99
 gosub handshake_send_byte
return
 /*SH1!1*/
 /*HS!!1*/get_gyro_data:
message_byte = 105
 gosub handshake_send_byte
gosub get_float_data
 phi_gyro = 1.26*PI*float_data/180
/ print "phi_gyro = ", 180*phi_gyro/PI
return
/*HH1!1*/reset_gyro:
 / gb 23.02.03 : gyro not used.
 / message_byte = 125
 / gosub handshake_send_byte
 /
 / float_data = 180*phi_wc/PI
 / gosub send_float_data
return
/*HH1!1*/get_battery_data:
 int b3,b2,b1
 message_byte = 106
 gosub handshake_send_byte
gosub get_float_data
return
/*HH1!1*/get_shaft_position_data:
 int b3,b2,b1
message_byte = 100
 gosub handshake_send_byte
// debug
 //gosub get_float_data
 //float rr = float_data
//gosub get_float_data
 //float ll = float_data
 // end debug
gosub get_float_data
 x_shaft = float_data
 /pak
 gosub get_float_data
 y_shaft = float_data
 /pak
 gosub get_float_data
 phi_shaft = PI*float_data/180
/print "x_shaft= ", x_shaft, ", y_shaft= ", y_shaft, ", phi_shaft= ", 180*phi_shaft/PI
return
 /*HH1!1*/reset_shaft_position:
message_byte = 120
 gosub handshake_send_byte
float_data = x_wc
 gosub send_float_data
float_data = y_wc
 gosub send_float_data
float_data = 180*phi_wc/PI
 gosub send_float_data
return
/*HH1!1*/reset_shaft_position_to_zero:
 x_wc = 0
 y_wc = 0
 gosub reset_shaft_position
 return
 /*SH1!1*/
 /*HS!!1*/float_to_three_bytes:
b3=0
 int Rest, Counter_base = 128
while float_data > 32000.0
 float_data -= 32000.0
 b3 += 1
 endwhile
while float_data < -32000.0
 float_data += 32000.0
 b3 += 1
 endwhile
Rest = int(float_data)
 if float_data < 0.0
 Rest = -int(float_data)
 b3 = b3 + Counter_base
 endif
b1 = land(Rest,255)
 b2 = Rest/256
// print " ---- b3= ", b3, " b2= ",b2, " b1= ", b1
return
/*HH1!1*/get_float_data:
gosub handshake_read_byte
 b3=in_byte
 gosub handshake_read_byte
 b2=in_byte
 gosub handshake_read_byte
 b1=in_byte
if b3 < 128
 float_data = 32000 * float(b3) + 256 * float(b2) + float(b1)
 endif
 if b3 >= 128
 float_data = - 32000 * float(b3-128) - 256 * float(b2) - float(b1)
 endif
//print " ---- b3= ", b3, " b2= ",b2, " b1= ", b1, " --> Float_data = ", float_data
return
/*HH1!1*/send_float_data:
gosub float_to_three_bytes
message_byte = b3
 gosub handshake_send_byte
message_byte = b2
 gosub handshake_send_byte
message_byte = b1
 gosub handshake_send_byte
return
/*HH1!1*/handshake_send_byte:
int in_byte, serial_count=0
 / print " "
in_byte = serial_byte_in(port)
 // just to empty the buffer of a possible ok_byte waiting
send_byte_loop:
 serial_count +=1
 serial_byte_out port, message_byte
 in_byte = serial_byte_in(port)
 / printf " %d,", in_byte
 if (in_byte != ok_byte) & (serial_count < serial_count_limit)
 goto send_byte_loop
 endif
if serial_count >= serial_count_limit
 print " "
 print "No response from RugWarrior while sending."
 print "Please press ESC, and reset the RugWarrior board."
 beep
 pak
 endif
 return
/*HH1!1*/handshake_read_byte:
int in_byte, in_byte1,serial_count=0
 / print " "
read_loop:
 serial_count += 1
 in_byte = serial_byte_in(port)
 in_byte1 = serial_byte_in(port)
 / printf " %d-%d,", in_byte, in_byte1
 if ((in_byte = 300) & (serial_count < serial_count_limit)) | (in_byte1 != in_byte )
 goto read_loop
 endif
 / printf " ->%d,", in_byte
if serial_count >= serial_count_limit
 print " "
 print "No response from RugWarrior while reading."
 print "Please reset it and check cables or power supply."
 beep
 pak
 endif
serial_byte_out port, ok_byte
 return
/*SH1!1*/
 /*HS!!1*/build:
 //dg 20,15,STANDARD_NODE_TYPE
 dg 40,30,STANDARD_NODE_TYPE
 //dg 54,40,STANDARD_NODE_TYPE
 mg contrast
 grid_nodestyle contrast, GREYSC_DISP_STYLE
 drawgrid contrast
mg Hough
 grid_nodestyle Hough, GREYSC_DISP_STYLE
 drawgrid Hough
dg 8,3, STANDARD_NODE_TYPE
 mg Average
 grid_nodestyle Average, GREYSC_DISP_STYLE
 drawgrid Average
 return
 /*HH121*/build_trajectories: // new incl circle diameter + number of elements
 float x1,y1,phi1, x1next, y1next, phi1next
 float theta, theta_next, r
 int nodes_id
/*HS!!2*/// neural net sizes definition
 / two circles
 section_size[1]=12
 section_size[2]=12
 section_size[3]=12
 section_size[4]=12
/inward spiral
 section_size[5]=12
 section_size[6]=10
 section_size[7]=8
 section_size[8]=5
 section_size[9]=5
/ four eights
 /section_size[10]=6
 /section_size[11]=6
 /section_size[12]=6
 /section_size[9]=6
section_size[10]=6
 section_size[11]=6
 section_size[12]=6
 section_size[13]=6
section_size[14]=6
 section_size[15]=6
 section_size[16]=6
 section_size[17]=6
section_size[18]=6
 section_size[19]=6
 section_size[20]=6
 section_size[21]=6
/ outward spiral
 /section_size[26]=3
 /section_size[27]=4
 section_size[22]=6
 section_size[23]=8
 section_size[24]=10
 section_size[25]=12
int net_size = 0
 //for i = 1 to 31
 //for i = 1 to 4 //31 : 1 -> 4 = only encode the two outer circles. Amsterdam
 / net_size += section_size[i]
 //next i
 // WELLCOME TRAJECTORY
 for i=1 to 31 // Wellcome
 if section_list[i]!=-99
 net_size+=section_size[section_list[i]]
 endif
 next i
 /*HH2!2*// nn build
dg 1,2, INPUT_NODE_TYPE
 mg Position
 grid_nodestyle Position, GREYSC_DISP_STYLE
 drawgrid Position
dg 12, int(float(net_size)/12)+1, GRBF_NODE_TYPE
 mg Trajectory
 grid_nodestyle Trajectory, GREYSC_DISP_STYLE
 drawgrid Trajectory
dg 1, 1, LINEAR_NODE_TYPE
 mg Sequence_State
 grid_nodestyle Sequence_State, GREYSC_DISP_STYLE
 drawgrid Sequence_State
/dg 1, 1, LINEAR_NODE_TYPE
 mg Special2
 grid_nodestyle Special2, GREYSC_DISP_STYLE
 drawgrid Special2
dg 1,4, LINEAR_NODE_TYPE
 mg Next_Position
 grid_nodestyle Next_Position, GREYSC_DISP_STYLE
 drawgrid Next_Position
connecttype ALL_TO_ALL
 connect Position, Trajectory
 connect Trajectory, Next_Position
 connect Trajectory, Sequence_State
set_weights 0
 set_grid_inweights Trajectory, 999999
 int section = 1
 int last_node = first_node(Trajectory)-1
 for nodes_id = last_node+1 to last_node+section_size[1]
 congridnode Sequence_State, nodes_id
 / set_weightsbetnodes first_node(Sequence_State), nodes_id, section_boost*(float(section)-0.5)
 next nodes_id
 last_node = last_node+section_size[1]
section=2
 for nodes_id = last_node+1 to last_node + section_size[2]
 congridnode Special2, nodes_id
 /set_weightsbetnodes first_node(Special2), nodes_id, section_boost*(float(section)-0.5)
 / old section based
 set_weightsbetnodes first_node(Special2), nodes_id, section_boost*(float(nodes_id - first_node(Trajectory) + 1)-0.5)
 / new node based
 next nodes_id
 last_node= last_node + section_size[2]
connect Sequence_State, Special2
 set_weightsbetgrids Sequence_State, Special2, 1
for nodes_id = last_node+1 to first_node(Trajectory)+net_size -1
 congridnode Sequence_State, nodes_id
 next nodes_id
uw
/*SH2!2*/// trajectory dimensions + margins
//r=350-margin of error due to low gain
 /float margin = 100
 / setting for gallery
float margin = 50
 / setting for Amsterdam 09 and Wellcome 2012
/float margin = 30
 / setting for drawing
float r0 = 300// 200 : a circle with a diam of 6m, for Wellcome Trust
 float dist_from_back_wall = 150 //not used?
 float xc= 0.5*max_room_xcm //center of circle
 //float yc= max_room_ycm - dist_from_back_wall - r - margin
 float yc=0.5*max_room_ycm+50 //center of circle
 //float yc=max_room_ycm - xc //iniva
 //float yc=410 //Amsterdam
 /*HS!!2*//first 1/2 circle, section 1
 last_node = first_node(Trajectory)-1
 section = 1
 float start_angle = PI
 float rotation_dir = 1
 // 1 = anticlockwise
 // opposite (clockwise) = -1
 r=r0-margin-10
 xc=0.5*max_room_xcm
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*//second 1/2 circle, section 2
 section = 2
 start_angle = 0
 // opposite (clockwise) = -1
 r=r0-margin-10
 xc=0.5*max_room_xcm
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*//third 1/2 circle, section 3
 section = 3
 start_angle = PI
 // opposite (clockwise) = -1
 r=r0-margin-10
 xc=0.5*max_room_xcm
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*//fourth 1/2 circle, section 4
 section = 4
 start_angle = 0
 // opposite (clockwise) = -1
 r=r0-margin-10
 xc=0.5*max_room_xcm
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*//spiral inward, section 5
 section = 5
 start_angle = PI
 // opposite (clockwise) = -1
 r=(5/6)*(r0-margin)
 xc=0.5*max_room_xcm-(0.5/6)*(r0-margin)
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//spiral inward, section 6
 section = 6
 start_angle = 0
 // opposite (clockwise) = -1
 r=(4/6)*(r0-margin)
 xc=0.5*max_room_xcm+(0.25/6)*(r0-margin)
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//spiral inward, section 7
 section = 7
 start_angle = PI
 // opposite (clockwise) = -1
 r=(3/6)*(r0-margin)
 xc=0.5*max_room_xcm-(0.5/6)*(r0-margin)
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//spiral inward, section 8 == 8
 section = 8
 start_angle = 0
 // opposite (clockwise) = -1
 r=(2.2/6)*(r0-margin)
 xc=0.5*max_room_xcm
 /*SH2!2*//gosub encode_half_circle
 /*HS!!2*//---spiral inward, section 9
 section = 9
 start_angle = PI
 // opposite (clockwise) = -1
 r=(1/6)*(r0-margin)
 xc=0.5*max_room_xcm-(0.5/6)*(r0-margin)
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//--- figure of 8, section 10
 section = 10
 start_angle = PI
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*//gosub encode_half_circle
 /*HS!!2*// ---figure of 8, section 11
 section = 11
 start_angle = 0
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// ---figure of 8, section 12
 section = 12
 start_angle = 0
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 13
 section = 13
 start_angle = PI
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=(1/2)*(r0-margin)
 xc=0.5*max_room_xcm-r
 //r=(1/6)*(r0-margin)
 //xc=0.5*max_room_xcm-r-(0.25/6)*(r0-margin)
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*// figure of 8, section 14
 section = 14
 start_angle = PI
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*// figure of 8, section 15
 section = 15
 start_angle = 0
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*// figure of 8, section 16
 section = 16
 start_angle = 0
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*/gosub encode_half_circle
 /*HS!!2*// figure of 8, section 17
 section = 17
 start_angle = PI
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 18
 section = 18
 start_angle = PI
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 19
 section = 19
 start_angle = 0
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 20
 section = 20
 start_angle = 0
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 21
 section = 21
 start_angle = PI
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 22
 section = 22
 start_angle = PI
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 23
 section = 23
 start_angle = 0
 rotation_dir = -1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm+r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 24
 section = 24
 start_angle = 0
 rotation_dir = 1
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*// figure of 8, section 25
 section = 25
 start_angle = PI
 // opposite (clockwise) = -1
 r=0.5*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//--spiral outward, section 26
 section = 26
 start_angle = 0
 // opposite (clockwise) = -1
 r=0.5*(1/4)*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*//gosub encode_half_circle
 /*HS!!2*//---spiral outward, section 27
 section = 27
 start_angle = PI
 // opposite (clockwise) = -1
 r=0.5*((1/4)+(1/3))*(r0-margin)
 xc=0.5*max_room_xcm
 /*HH2!2*//gosub encode_half_circle
 /*HH2!2*//spiral outward, section 28 == 22
 section = 28
 start_angle = 0
 // opposite (clockwise) = -1
 r=(2/7)*(r0-margin)
 /r=0.5*((2/4)+(1/3))*(r0-margin)
 xc=0.5*max_room_xcm-r
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//spiral outward, section 29 == 23
 section = 29
 start_angle = PI
 // opposite (clockwise) = -1
 r=0.5*((2/4)+(2/3))*(r0-margin)
 xc=0.5*max_room_xcm
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*//spiral outward, section 30 == 24
 section = 30
 start_angle = 0
 // opposite (clockwise) = -1
 /r=(4/6)*(r0-margin)
 r=0.5*((3/4)+(2/3))*(r0-margin)
 xc=0.5*max_room_xcm-(0.5/6)*(r0-margin)
 /*HH2!2*///gosub encode_half_circle
 /*HH2!2*//spiral outward, section 31 == 25
 section = 31
 start_angle = PI
 // opposite (clockwise) = -1
 //new 1/2 circle
 //r=r0-margin-10
 //xc=0.5*max_room_xcm
 //old 1/2 spiral out
 r=0.5*((3/4)+(3/3))*(r0-margin)
 xc=0.5*max_room_xcm+(0.25/6)*(r0-margin)
 /*SH2!2*///gosub encode_half_circle
 /*HS!!2*/// set reciprocal weights (trajectory <-> Sequence_State)
int section, last_node=first_node(Trajectory)-1
 float previous_phase = 0, current_phase = 0
//for section = 1 to 31
 //for section = 1 to 4 //31 // only 2 outer circles. Amsterdam
 for i = 1 to 31
 section = section_list[i]
 if section!=-99
for nodes_id = last_node+1 to last_node + section_size

/float node_sigma = GRBF_node_sigma(nodes_id)
 /previous_phase = wire_weight(wirebetnodes(nodes_id, first_node(Sequence_State)))
 /current_phase = previous_phase + node_sigma*section_boost*float(nodes_id - first_node(Trajectory) + 1)
 /set_weightsbetnodes nodes_id, first_node(Sequence_State), current_phase
 / very new node+sigma based
set_weightsbetnodes nodes_id, first_node(Sequence_State), section_boost*float(nodes_id - first_node(Trajectory) + 1)
 / new node based
 /set_weightsbetnodes nodes_id, first_node(Sequence_State), section_boost*float(section)
 / old section based
 if section != 2
 // section 2 done above
/current_phase = previous_phase + node_sigma*section_boost*(float(nodes_id - first_node(Trajectory) + 1)-0.5)
 /set_weightsbetnodes first_node(Sequence_State), nodes_id, current_phase
 / very new node+sigma based
 /set_weightsbetnodes first_node(Sequence_State), nodes_id, section_boost*(float(section)-0.5)
 / old section based
 set_weightsbetnodes first_node(Sequence_State), nodes_id, section_boost*(float(nodes_id - first_node(Trajectory) + 1)-0.5)
 / new node based
 endif
 next nodes_id
 last_node = last_node + section_size

endif
 //print "section done = ", section, " last_node = ", last_node
 //pak
 next i
 //next section
return
 /*HH1!1*/encode_half_circle: // new: sigma set here
 /if section > 0
 for i = 0 to section_size
-1
theta = start_angle+rotation_dir*float(i)*PI/float(section_size
)
 theta_next = start_angle+rotation_dir*float(i+1)*PI/float(section_size
)
x1=r*cos(theta)+xc
 y1=r*sin(theta)+yc
 phi1 = theta+PI/2
x1next=r*cos(theta_next)+xc
 y1next=r*sin(theta_next)+yc
 phi1next = theta_next+(PI/2)*(rotation_dir) //gb
/print "Theta = ", theta
 /pak
 nodes_id = last_node +i+1
set_weightsbetnodes first_node(Position), nodes_id, x1
 set_weightsbetnodes first_node(Position)+1, nodes_id, y1
set_weightsbetnodes nodes_id, first_node(Next_Position), x1next
 set_weightsbetnodes nodes_id, first_node(Next_Position)+1, y1next
 // set_weightsbetnodes nodes_id, first_node(Next_Position)+2, phi1next
 set_weightsbetnodes nodes_id, first_node(Next_Position)+2, sin(phi1next)
 set_weightsbetnodes nodes_id, first_node(Next_Position)+3, cos(phi1next)
//print "node= ",nodes_id
 /print i, " , ", 180*phi1next/PI
 //pak
/set_GRBF_node_sigma nodes_id, max_room_xcm/15
 // set_GRBF_node_sigma nodes_id, 25+r/5 //amsterdam
 set_GRBF_node_sigma nodes_id, 25+r/10 //wellcome
 / for field drawing
 / set_GRBF_node_sigma nodes_id, 20+r/5
 / for trajectory drawing
/ set_GRBF_node_sigma nodes_id, r/3
 / set_GRBF_node_sigma nodes_id,
 / float(nodes_id - first_node(Trajectory) + 1)
next i
last_node = nodes_id
 //print "section done= ", section, " last_node = ", last_node
 //pak
/endif
 return
 /*HH1!1*/build_a_circle:
 return
 /*HH121*/update_network2: //new. Deals with closing the loop
set_node_output first_node(Position), x_wc_predict
 set_node_output first_node(Position)+1, y_wc_predict
 /set_node_output first_node(Position)+2, phi_wc_predict
updategrid Trajectory
 int current_grid = Trajectory
 gosub normalize_winner
 / gosub lateral_inhibition
gosub update_Sequence_State
 / commented out for field drawing
current_section = int(node_output(first_node(Sequence_State))/section_boost)
 print "======> Section nb = ", node_output(first_node(Sequence_State))/section_boost, " <====="
updategrid Special2
/*HS!!2*/ // dealing with loop back to first state
// three states before the last -> switch to the next section
 // (explains the 2.5 and 3 below)
 if node_output(first_node(Sequence_State)) >= (float(net_size)-2.5)*section_boost
 // from the last state, jump to the 12th. Old version.
 // avoiding section 1. Unclear why.
 //set_grid_outputs Special2, 13*section_boost
 //set_grid_outputs Sequence_State, 13*section_boost
 //try top jump to the start of section 1. Amsterdam 09
 forced_next_state= 3-float(net_size)+(node_output(first_node(Sequence_State))/section_boost)
 print "forced_next state = ", forced_next_state
 //print "current state = ", node_output(first_node(Sequence_State))/section_boost
 //pak
 set_grid_outputs Special2, forced_next_state*section_boost
 set_grid_outputs Sequence_State, forced_next_state*section_boost
 updategrid Trajectory
 / new node based
 endif
/print "-----Special2 = ", node_output(first_node(Special2))
 gosub update_Next_Position
/ drawgrid Trajectory, Next_Position, Position, Sequence_State, Special2
 //ug
 return
/*HH1!1*/update_Next_Position: // new
 float act_sum = 0
 // NOTE: take care when the circle closes , 360 = 0 (but av = 180!)
updategrid Next_Position
 //pak
 for k = first_node(Trajectory) to last_node(Trajectory)
 let act_sum = act_sum + node_output(k)
 //printf " %2.20f,", act_sum
 next k
 //print "act_sum = ", act_sum
 //printf "\n"
/ done already in update_Sequence_State called earlier
if act_sum > 0.0
 for k = first_node(Next_Position) to last_node(Next_Position)
 //print "node= ", k, " node_output= ", node_output(k)
 set_node_output k, node_output(k)/act_sum
 next k
 // use sin and cos to calculate the next angle put into the 3rd node.
float sind = node_output(first_node(Next_Position)+2)
 float cosd = node_output(first_node(Next_Position)+3)
/print "sind = ", sind, " cosd= ", cosd
 float delta_phi = arctan(sind/cosd)
if sind >=0 & cosd <0
 delta_phi += PI
 endif
 if sind < 0 & cosd < 0
 delta_phi -= PI
 endif
set_node_output first_node(Next_Position)+2 , delta_phi
else
 print "low activity limit"
 / pak
 endif
 return
 /*HH1!1*/update_Sequence_State: // new
 float act_sum = 0
 /float act1 = node_output(first_node(Sequence_State))
updategrid Sequence_State
 float act1 = node_output(first_node(Sequence_State))
 for k = first_node(Trajectory) to first_node(Trajectory) + net_size-1
 let act_sum = act_sum + node_output(k)
 next k
 /pak
 k = first_node(Sequence_State)
 / if act_sum > 0.000001
 if act_sum > 0.0
 set_node_output k, node_output(k)/act_sum
 /my_sound 100,100
 else
 /pak
 set_node_output k, act1
endif
 return
 /*HH1!1*/angle_diff:
float sind=(cos(angle2)*sin(angle1)-sin(angle2)*cos(angle1))
 float cosd = (cos(angle2)*cos(angle1)+sin(angle2)*sin(angle1))
 float local_tan = sind/cosd
 float angle_difference = arctan(local_tan)
if sind >=0 & cosd <0
 angle_difference += PI
 endif
 if sind < 0 & cosd < 0
 angle_difference -= PI
 endif
 angle_difference =-angle_difference
 return
/*SH1!1*/
 /*HS!!1*/get_sonar_data: //for self localization -used?
 //graph_active Sonar
 //graph_clear
keyboard = port_in(96)
 // NOTE: Somehow this read fools the power saving system
 // as if a key had been pressed and there is no more slowing
 // down of the data reading after a few seconds.
 // port 96 is the keyboard chip 8255A (hex 60).
 //print keyboard
/ ms_delay 40
 sonar = 3
 gosub fast_get_sonar_range
 / ms_delay 40
sonar = 1
 gosub fast_get_sonar_range
 / ms_delay 40
sonar = 5
 gosub fast_get_sonar_range
 / ms_delay 40
sonar = 7
 gosub fast_get_sonar_range
 / ms_delay 40
/for sonar = 1 to 8
 / gosub fast_get_sonar_range
 /next sonar
sonar_d1 = sonar_range[1] - sonar_y[1]
 sonar_d2 = sonar_range[3]- sonar_x[3]
 sonar_d3 = sonar_range[5]+ sonar_y[5]
 sonar_d4= sonar_range[7]+ sonar_x[7]
 return
 /*HH1!1*/fast_get_sonar_range: // ibm thinkpad
// sonar_range[sonar]=200
// gosub get_sonar_range //gb test
 //return
ms_delay 30 //20 ...10ms for 3m
 int mask = 2^(sonar-1)
 int echo_on = 1
 int iix, iiy
port_out portb, mask
/======================
 //ms_delay 1
 port_out porta, mask
for i = 0 to max_sonar_array-1
 echo[i]=0 //gb 130811
 if echo_on =1
 //echo[i]=port_in(portb)
 for iix=1 to 62 // 50 //to create a delay between port_in calls
 iiy+=iix // needed by ibm thinkpad
 next iix //this delay also shifts the palce of the echo in the array.
 // with 62 ittreaqtio and the factor 1.42, we get the right distance...
 // with 50 ittreaqtio and the factor 1.17, we get the right distance...
 echo[i]=port_in(portb)
 endif
 if echo[i] = mask & echo_on=1
 echo_on=0
 echo[i]=port_in(portb) //run once
 endif
 if echo_on=0
 echo[i]=mask //to simulate the rest of the inputs
 endif
 //print echo[i]
 next i
ms_delay 1
port_out porta,0
 /=====================
int i_dist = -1
 for i = min_range_check to max_sonar_array-1
if echo[i] = mask
 if i_dist = -1
 i_dist = i
 i = max_sonar_array
 endif
 endif
 next i
sonar_range[sonar]=float(i_dist)*1.42 // 1.17 //1.19
 //print "sonar = ", sonar, " range = ", sonar_range[sonar]
 return
 /*HH1!1*/sonar_scan8:
while 1
 graph_active Sonar
 graph_clear
 keyboard = port_in(96)
for sonar = 1 to 8
 //sonar = 5
 gosub get_sonar_range
 //gosub get_sonar_range_old
 //ms_delay 400 //50
 next sonar
 endwhile
 return
 /*HH1!1*/get_sonar_range: //new test stop reading echo when echo received, gb 27.6.2012
 float dt
 //sonar = 5 //gbtest
int mask = 2^(sonar-1)
 //int mask = 2^(5-1) //gbtest only read from 5 - in front
 int echo_on = 1
 int iix, iiy
 ms_delay 30 //gb 10ms = 3m
 port_out portb, mask //reading address
 //ms_delay 1
/init_timer
 /======================
 //int mask = 2^(sonar-1) //gbtest
port_out porta, mask //ping out
for i = 0 to max_sonar_array-1
 echo[i]=0 //gb 130811
 if echo_on =1
 //echo[i]=port_in(portb)
 for iix=1 to 50 //1 //to create a delay between port_in calls
 iiy+=iix // needed by ibm thinkpad
 next iix //this delay also shifts the palce of the echo in the array.
 // with 61 ittreaqtio and the factor 1.386, we get the right distance...
 // with 62 ittreaqtio and the factor 1.42, we get the right distance...
 // with 50 ittreaqtio and the factor 1.17, we get the right distance...
 echo[i]=port_in(portb)
 endif
 if echo[i] = mask & echo_on=1
 echo_on=0
 echo[i]=port_in(portb) //run once more
 endif
 if echo_on=0
 echo[i]=mask //to simulate the rest of the inputs
 endif
 //print echo[i]
 next i
 ms_delay 1
port_out porta,0 // reset all inputs?
/=====================
 /dt = elapsed_time(1)
/-----
 int i_dist = 0
 /init_timer
 for i = min_range_check to max_sonar_array-1
color = GREEN
 / if land(echo[i],mask) = 0
 if echo[i]=0
 color = RED
 endif
/ if land(echo[i],mask) = mask
 if echo[i] = mask
 color = YELLOW
 if i_dist = 0
 i_dist = i
 / i = max_sonar_array
 endif
 endif
move float(i), float(sonar+1)-0.3
 line float(i), float(sonar+1)+0.3, color
next i
//print "Sonar ", sonar, ", range = ",i_dist, " (dt1=",dt," dt2=",elapsed_time(1),")"
 print "Sonar ", sonar, ", range [cm] = ",float(i_dist)*1.17 //1.389
 if sonar = 8
 print " "
 endif
 sonar_range[sonar]=float(i_dist)*1.17 // 1.19
 return
 /*HH1!1*/get_sonar_range_old:
 float dt
 int mask = 2^(sonar-1)
 int iix, iiy
port_out portb, mask
/init_timer
 /======================
 port_out porta, mask
 for i = 0 to max_sonar_array-1
 echo[i]=0 //gb 130811
 echo[i]=port_in(portb)
 for iix=1 to 10 //to create a delay between port_in calls
 iiy+=iix // needed by ibm thinkpad
 next iix
 //print echo[i]
 next i
port_out porta,0
 /=====================
 /dt = elapsed_time(1)
/-----
 int i_dist = 0
 /init_timer
 for i = min_range_check to max_sonar_array-1
color = GREEN
 / if land(echo[i],mask) = 0
 if echo[i]=0
 color = RED
 endif
/ if land(echo[i],mask) = mask
 if echo[i] = mask
 color = YELLOW
 if i_dist = 0
 i_dist = i
 / i = max_sonar_array
 endif
 endif
move float(i), float(sonar+1)-0.3
 line float(i), float(sonar+1)+0.3, color
next i
//print "Sonar ", sonar, ", range = ",i_dist, " (dt1=",dt," dt2=",elapsed_time(1),")"
print "Sonar ", sonar, ", range [cm] = ",float(i_dist)*1.389
 if sonar = 8
 print " "
 endif
 sonar_range[sonar]=float(i_dist)*1.19
 //sonar_range[sonar]=float(i_dist)*1.19 //values for old laptop
 //endif
 return
 /*HH1!1*/sonar_plot:
 /-------sonar plot------------
 // ------------ erase old data-------------
 // printf "x_wc= %f, y_wc= %f, phi_wc= %f\n", x_wc,y_wc,phi_wc
 // print "phi_wc = ",phi_wc*180/PI
 color = 0
 // 0 = black
 for sonar = 1 to 8
 xs = old_x_wc + sonar_x[sonar]*cos(old_phi_wc-0.5*PI) - sonar_y[sonar]*sin(old_phi_wc-0.5*PI)
 ys = old_y_wc + sonar_y[sonar]*cos(old_phi_wc-0.5*PI) + sonar_x[sonar]*sin(old_phi_wc-0.5*PI)
 win_move xs,ys
 xs = xs + old_sonar_range[sonar]*cos(old_phi_wc-0.5*PI + sonar_dir[sonar])
 ys = ys + old_sonar_range[sonar]*sin(old_phi_wc-0.5*PI + sonar_dir[sonar])
 win_line xs,ys, color
 next sonar
 //print "erased old sonar"
 //pak
 // -------------new data-------------
 color = 5
 // a level of grey
 for sonar = 1 to 8
 xs = x_wc + sonar_x[sonar]*cos(phi_wc-0.5*PI) - sonar_y[sonar]*sin(phi_wc-0.5*PI)
 ys = y_wc + sonar_y[sonar]*cos(phi_wc-0.5*PI) + sonar_x[sonar]*sin(phi_wc-0.5*PI)
 win_move xs,ys
 xs = xs + sonar_range[sonar]*cos(phi_wc -0.5*PI + sonar_dir[sonar])
 ys = ys + sonar_range[sonar]*sin(phi_wc -0.5*PI + sonar_dir[sonar])
 win_line xs,ys, color
 old_sonar_range[sonar] = sonar_range[sonar]
 next sonar
 /pak
 return
 /*SH1!1*/
 /*SS!!1*/
 /*HS!!1*/find_min_max_act:
 float max_act = 0
 float min_act = 1
 int max_node=0, min_node=0
 for nodes_id = first_node(current_grid) to last_node(current_grid)
 if node_output(nodes_id) > max_act
 max_act = node_output(nodes_id)
 max_node = nodes_id
 endif
 if node_output(nodes_id) < min_act
 min_act = node_output(nodes_id)
 min_node = nodes_id
 endif
 next nodes_id
return
/*HH1!1*/normalize_winner:
 // boost all by factor such that the winner has
 // an activity 1 and the smallest an activity 0
gosub find_min_max_act
// boost
 for nodes_id = first_node(current_grid) to last_node(current_grid)
 set_node_output nodes_id, (node_output(nodes_id)-min_act)/(max_act-min_act)
 next nodes_id
return
/*HH1!1*/lateral_inhibition:
 // inhibit all by average activity of active nodes
 // find average
 float av_act = 0
 float node_number = 0
 for nodes_id = first_node(current_grid) to last_node(current_grid)
 if node_output(nodes_id) > 0.01
 av_act = av_act + node_output(nodes_id)
 node_number = node_number + 1
 endif
 next nodes_id
 av_act = 0.8*av_act/node_number
 // to prevent problems when all active nodes have the same output
if node_number = 1
 av_act = 0
 endif
 // subtract average
 for nodes_id = first_node(current_grid) to last_node(current_grid)
 if node_output(nodes_id) > av_act
 set_node_output nodes_id, node_output(nodes_id) - av_act
 else
 set_node_output nodes_id, 0
 endif
 next nodes_id
 return
 /*SH1!1*/
 /*#CP=29256:XX=99*/