# This source was originally 'explorateur.aesl', but code has been added to react to RC5 commands, as produced # for example by Philips TV remote controls. # The commands which are effective are as follows: # Cursor up (RC5 code=80) : go forwards, or go faster # Cursor down (RC5 code=81): go backwards, or go faster. N.B. turning does not function when going backward # Cursor left (RC5 code=85) : turn to the left by a fixed amount (see note below) # Cursor right (RC5 code=86) : turn to the right by a fixed amount (see note below) # numeric 2 : go forwards, or go faster # numeric 8 : go backwards, or go faster. N.B. turning does not function when going backward # numeric 4 : turn to the left by a fixed amount (see note below) # numeric 6 : turn to the right by a fixed amount (see note below) # numeric 5 : stop # Volume Up (RC5 code=16) : double the amount turned # Volume Down (RC5 code=17) : halve the amount turned # On/Off (red) button (RC5 code=12) : reset amount turned to starting value # Notes: # # The RC5 codes for the numerics are their values (i.e. key 2 : 2, key 5 : 5) # # The amount turned is initially about 30 degrees with the LOOPCOUNT_T constant set to 10000 # # Changes were made to the object avoidance code. # In particular, if an obstacle is detected while going backward, STOP! # This is because due to there being only two rearward facing sensors, # the writer of these lines was not able to think of how to take appropriate avoidance action in this case, # unlike when going forward, with 5 sensors, 4 of which are angled outward. var temp var temp2 var speed=200 var speed_t=0 var vmin=-600 var vmax=600 var l[8] var led_state=0 var fixed var led_pulse var prox_sensor_2_last=-1 var index_i var index_j var wait_loop_ct=500 var dummysum var loop_count_t=LOOPCOUNT_T sub faster speed=speed+SPEED_INC if speed>SPEED_MAX_FWD then speed=SPEED_MAX_FWD end sub slower speed=speed-SPEED_INC if speed<SPEED_MAX_BKWD then speed=SPEED_MAX_BKWD end sub stop speed=0 speed_t=0 motor.left.target=0 motor.right.target=0 sub turnleft speed_t = -speed/SPEED_T_FACTOR motor.left.target=speed+speed_t motor.right.target=speed-speed_t # now count the time to keep turning dummysum=0 index_i = 1 while index_i <loop_count_t do dummysum=dummysum+1 index_i = index_i + 1 end speed_t=0 sub turnright speed_t = -speed/SPEED_T_FACTOR motor.left.target=speed-speed_t motor.right.target=speed+speed_t # now count the time to keep turning dummysum=0 index_i = 1 while index_i <loop_count_t do dummysum=dummysum+1 index_i = index_i + 1 end speed_t=0 sub inc_angle loop_count_t = 3*(loop_count_t/2) if loop_count_t <0 then # overflow (> 32767) loop_count_t = LOOPCOUNT_MAX end index_j = 1 while index_j <wait_loop_ct do # wait a little while index_j = index_j + 1 end sub dec_angle loop_count_t = (loop_count_t/3) * 2 if loop_count_t <50 then loop_count_t = 50 end index_j = 1 while index_j <wait_loop_ct do # wait a little while index_j = index_j + 1 end onevent buttons when button.forward==1 do # increase speed callsub faster end when button.backward==1 do # decrease speed callsub slower end when button.center==1 do # stop robot callsub stop end when button.left==1 do # turn left if speed>0 then callsub turnleft end end when button.right==1 do # turn right if speed>0 then callsub turnright end end #Led ring animation call math.fill(l, 0) led_state = led_state + 2 if led_state> 255 then led_state=0 end fixed = led_state /32 l[fixed] = 32 l[(fixed - 1) & 0x7] = 32 - (led_state & 0x1F) l[(fixed + 1) & 0x7] = led_state & 0x1F call leds.circle(l[0], l[1], l[2], l[3], l[4], l[5], l[6], l[7]) #Body color pulse led_pulse = led_pulse + 1 if led_pulse > 0 then call leds.top(led_pulse,0 , led_pulse) if led_pulse > 40 then led_pulse = -64 end else temp=-led_pulse/2 call leds.top(temp, 0, temp) # R + B = purple end onevent prox # Braitenberg obstacle avoidance if speed >0 then # temp contains weighted sum of all 5 forward facing sensor values (weight = 1 2 3 2 1 ) temp=(prox.horizontal[0]*1+prox.horizontal[1]*2+prox.horizontal[2]*3+prox.horizontal[3]*2+prox.horizontal[4]*1) # temp2 contains weighted sum of the 2 left and 2 right sensor values (weight = -4 -3 3 4 ) temp2=prox.horizontal[0]*-4+prox.horizontal[1]*-3+prox.horizontal[3]*3+prox.horizontal[4]*4 # the problem is that when the obstacle is directly in front of the robot, only prox.horizontal[2] has a value and then # the robot just repeatedly moves forward and backward because temp2 will contain zero # the following is an attempt to correct this and it seems to do the trick! if (prox.horizontal[1] == 0) and (prox.horizontal[3] == 0) and ((prox.horizontal[2] - prox_sensor_2_last) > PROX_INC) then temp2 = PROX_INC2 end prox_sensor_2_last = prox.horizontal[2] motor.left.target=speed-(temp+temp2)/(2000/speed) motor.right.target=speed-(temp-temp2)/(2000/speed) elseif speed <0 then # N.B. avoidance not working well when going backwards so changed so as to stop, which works ******* # prox index 5 = back left, 6 = back right if (prox.horizontal[5] + prox.horizontal[6]) > REV_SENS_MAX then # backwards into wall, so stop motor.left.target=0 motor.right.target=0 speed = 0 else # this code is never reached, I believe # if 0 > speed > -500, temp will vary from very large to less than 1 temp=-REVERSE_OBST_FACTOR/speed # prox index 5 = back left, 6 = back right if (temp > 0) or (temp < 0) then # do not divide by zero motor.left.target=speed+prox.horizontal[6]/temp motor.right.target=speed+prox.horizontal[5]/temp end end call math.min(motor.left.target, motor.left.target, vmax) call math.min(motor.right.target, motor.right.target, vmax) call math.max(motor.left.target, motor.left.target, vmin) call math.max(motor.right.target, motor.right.target, vmin) end # Note that the two downward-facing sensors are at the front of Thymio, so the following code is not effective when # Thymio is going backwards! You have been warned! #Detect table edge if prox.ground.reflected[0]<130 or prox.ground.reflected[1]<130 then motor.left.target=0 motor.right.target=0 call leds.bottom.left(32,0,0) # Red call leds.bottom.right(32,0,0) else call leds.bottom.left(0,0,0) call leds.bottom.right(0,0,0) end onevent rc5 if rc5.command !=0 then if rc5.command == UP_RC or rc5.command == CURSOR_UP_RC then callsub faster elseif rc5.command == DOWN_RC or rc5.command == CURSOR_DOWN_RC then callsub slower elseif rc5.command == RIGHT_RC or rc5.command == CURSOR_RIGHT_RC or rc5.command == CURSOR_RIGHT_ALT_RC then if speed>0 then callsub turnright end elseif rc5.command == LEFT_RC or rc5.command == CURSOR_LEFT_RC or rc5.command == CURSOR_LEFT_ALT_RC then if speed>0 then callsub turnleft end elseif rc5.command == STOP_RC then callsub stop elseif rc5.command == VOL_UP_RC then callsub inc_angle elseif rc5.command == VOL_DOWN_RC then callsub dec_angle elseif rc5.command == ON_OFF_RC then loop_count_t = LOOPCOUNT_T # reset turn factor end rc5.command = 0 end