diff --git a/.rvmrc b/.rvmrc index 4c79880..b86def2 100644 --- a/.rvmrc +++ b/.rvmrc @@ -1,5 +1,5 @@ -if [ -f "$0".local ]; then - source "$0".local +if [ -f .rvmrc.local ]; then + source .rvmrc.local else rvm use 1.9.3 fi diff --git a/Gemfile b/Gemfile index aae912e..47002e4 100644 --- a/Gemfile +++ b/Gemfile @@ -20,6 +20,11 @@ group :assets do gem 'twitter-bootstrap-rails' end +group :development,:test do + gem 'debugger' + gem 'ein' #physics simulator +end + group :development do gem 'thin' end diff --git a/Gemfile.lock b/Gemfile.lock index 07b980d..338d13d 100644 --- a/Gemfile.lock +++ b/Gemfile.lock @@ -39,8 +39,17 @@ GEM coffee-script-source execjs coffee-script-source (1.2.0) + columnize (0.3.6) commonjs (0.2.5) daemons (1.1.8) + debugger (1.2.3) + columnize (>= 0.3.1) + debugger-linecache (~> 1.1.1) + debugger-ruby_core_source (~> 1.1.5) + debugger-linecache (1.1.2) + debugger-ruby_core_source (>= 1.1.1) + debugger-ruby_core_source (1.1.6) + ein (0.0.2) erubis (2.7.0) eventmachine (0.12.10) execjs (1.3.0) @@ -139,6 +148,8 @@ PLATFORMS DEPENDENCIES coffee-rails + debugger + ein jquery-rails json minitest diff --git a/README.md b/README.md index 5ce4f3d..8c3415f 100644 --- a/README.md +++ b/README.md @@ -29,17 +29,23 @@ Jump in console and give the basic simulation a shot. ````ruby $ rails c -:001 > earth = World.new -:002 > roo = RoombaSimulation.new -:003 > earth.spawn(roo) -:004 > roo.move(100) -:005 > roo.move(0,120) -:006 > roo.move(1000) +:001 > simulator = Simulator.new +:002 > roo = RoombaSimulation.new(simulator) +:004 > simulator.start +:005 > roo.move(100) +:006 > roo.move(0,90) +:007 > roo.move(1000) ```` You should end up with a bump reading at N:90, X:126 Y:89 X:126 is the center point of Simulated Roomba. Add the radius of Roomba + the radius of the obstacle and it should be the same as the distance between X:126 and the default simulated obstacle. +If you want to get information of each step of the simulation do: + +```Ruby +LOGGER.level = Logger::DEBUG +``` + Or jump into the rails app and play around. ```` diff --git a/app/controllers/simulations_controller.rb b/app/controllers/simulations_controller.rb index 74cd7ce..72f7943 100644 --- a/app/controllers/simulations_controller.rb +++ b/app/controllers/simulations_controller.rb @@ -13,12 +13,15 @@ def index # GET /simulations/1 # GET /simulations/1.json def show + #TODO: serialize/deserialize sim from DB @simulation = Simulation.find(params[:id]) + # TODO: This is confusing + # Watch out! Simulator != Simulation . + simulator = Simulator.new + @world = simulator.world #this is super crap, just for debugging ATM, moving out soon - @world = World.new - @roombot = RoombaSimulation.new - @world.spawn(@roombot) + @roombot = RoombaSimulation.new(simulator) respond_to do |format| format.html # show.html.erb diff --git a/config/application.rb b/config/application.rb index f17f11d..f6151c7 100644 --- a/config/application.rb +++ b/config/application.rb @@ -17,7 +17,7 @@ class Application < Rails::Application # Custom directories with classes and modules you want to be autoloadable. # config.autoload_paths += %W(#{config.root}/extras) - config.autoload_paths += %W(#{config.root}/lib) + config.autoload_paths += %W(#{config.root}/lib #{config.root}/lib/simulator) # Only load the plugins named here, in the order given (default is alphabetical). # :all can be used as a placeholder for all plugins not explicitly named. diff --git a/lib/byte_processing.rb b/lib/byte_processing.rb new file mode 100644 index 0000000..48f63ee --- /dev/null +++ b/lib/byte_processing.rb @@ -0,0 +1,13 @@ +module ByteProcessing + + def signed_integer(bytes) + case bytes.size + when 1 + return (bytes[0] & ~(1 << 7)) - (bytes[0] & (1 << 7)) + when 2 + sixteenbit = bytes[0] << 8 | bytes[1] + return (sixteenbit & ~(1 << 15)) - (sixteenbit & (1 << 15))#http://en.wikipedia.org/wiki/Two%27s_complement#Calculating_two.27s_complement + end + end + +end diff --git a/lib/calculations.rb b/lib/calculations.rb new file mode 100644 index 0000000..7377834 --- /dev/null +++ b/lib/calculations.rb @@ -0,0 +1,12 @@ +module Calculations + def calculate_spin_time(velocity, degree) + # time = wheelbase * PI / 360degrees * degrees / velocity ABS + # wheelbase might be different for different roombas, consider refactoring + ((((Roomba::Specification::WHEELBASE * Math::PI) / 360) * degree.abs).to_f / velocity.to_f).abs + end + + #spinning needs some work + def calculate_spin_degree(velocity, time) + ((time.to_f * velocity.to_f) / ((Roomba::Specification::WHEELBASE * Math::PI) / 360)) #/ 10**10 + end +end diff --git a/lib/roomba.rb b/lib/roomba.rb index 4c07e45..4f32829 100644 --- a/lib/roomba.rb +++ b/lib/roomba.rb @@ -19,6 +19,8 @@ class Roomba :saturday => '01000000' } + include ByteProcessing + include Calculations def initialize(port, latency=0.1, baud=115200, serial=nil) # baud must be 115200 for communicating with 500 series Roomba and newer (tested with Roomba 770), change to 57600 for 400 series and older @@ -65,9 +67,9 @@ def move(distance, degree=0, velocity=200) set_velocity(velocity) set_degree(degree) drive(@velocity_high, @velocity_low, @radius_high, @radius_low) - start_moving = Time.now time_in_seconds = 10 if time_in_seconds > 10 - until (start_moving - Time.now).abs >= time_in_seconds + start_moving = current_time + until (start_moving - current_time).abs >= time_in_seconds # sensors call sleeps the script for 20ms, max read is 50ms, total time between loops about 65ms sensors = get_readings(:bumps_and_drops, :wall) @messages.push sensors @@ -77,15 +79,10 @@ def move(distance, degree=0, velocity=200) sensors end - def calculate_spin_time(velocity, degree) - # time = wheelbase * PI / 360degrees * degrees / velocity ABS - # wheelbase might be different for different roombas, consider refactoring - ((((WHEELBASE * Math::PI) / 360) * degree.abs).to_f / velocity.to_f).abs - end - - #spinning needs some work - def calculate_spin_degree(velocity, time) - ((time.to_f * velocity.to_f) / ((WHEELBASE * Math::PI) / 360)) / 10**10 + #This is overwritten in the roomba_simulation so we can use simulation + #time instead of real world time. + def current_time + Time.now end def set_degree(degree) @@ -166,7 +163,7 @@ def get_readings(*sensors_requested) readings[sensor] = {:raw => nil, :formatted => []} readings[sensor][:raw] = bytes.shift(SENSORS[sensor][:bytes]) readings[sensor][:formatted] = set_readings(sensor, readings[sensor][:raw]) - puts "Sensors: #{readings[sensor].inspect}" + #puts "Sensors: #{readings[sensor].inspect}" end readings #return hash of readings end @@ -189,16 +186,6 @@ def set_readings(sensor, readings) end end - def signed_integer(bytes) - case bytes.size - when 1 - return (bytes[0] & ~(1 << 7)) - (bytes[0] & (1 << 7)) - when 2 - sixteenbit = bytes[0] << 8 | bytes[1] - return (sixteenbit & ~(1 << 15)) - (sixteenbit & (1 << 15))#http://en.wikipedia.org/wiki/Two%27s_complement#Calculating_two.27s_complement - end - end - def motors motors(1) sleep 2 diff --git a/lib/roomba_serial_simulation.rb b/lib/roomba_serial_simulation.rb index d760edb..348a778 100644 --- a/lib/roomba_serial_simulation.rb +++ b/lib/roomba_serial_simulation.rb @@ -1,98 +1,117 @@ -class RoombaSerialSimulation < Roomba - attr_accessor :simulation, :requested_readings, :readings, :x, :y, :facing, :moving, :velocity, :turning, :degree, :timestamp, :world +class RoombaSerialSimulation + + include ByteProcessing + include Calculations # currently the simulation settings are hardcoded in the initializer # need to refactor to allow various predefined or even random simulations - def initialize - yield self if block_given? - - # Set defaults if not set in the initializer block - # These defaults match the previously hard-coded values - @simulation ||= 'simulation' - @x ||= 0 - @y ||= 0 - @facing ||= 0 #+y, @facing of 90 == +x, @facing of 180 == -y, @facing of 270 == -x - + def initialize(sensors=nil) # The following are not to be set by the user - @moving = false @velocity = 0 @degree = 0 @turning = false @readings = [] + @bumpers = sensors #bumpers are the only sensors we have now + @waiting_bytes = 0 + @command_bytes = [] self end - # When the RoombaSimulation class tries to send data to the simulated Roomba - # this is where that data arrives. Check the first byte to get the opcode - # if a method exists for handling that opcode, run it; Need to write mock - # methods for each useful ROI command + # this is where that data arrives. + # Roomba will write one byte at a time here + # we need to take all the bytes and reconstruct the original order + # This method will be highly simplify when we abstract from the byte + # protocoll of Roomba to a more general interface. + # Need to write mock methods for each useful ROI command + def write(*bytes) - puts "Bytes Roomba received: #{bytes.inspect}" - command = bytes.shift - case command - when 137 - move(*bytes) - when 149 - prepare_readings(*bytes) + byte = bytes.first.ord + if @waiting_bytes == 0 + command = byte + @command_bytes = [command] + @waiting_bytes = bytes_needed_per_command(command) + else + @command_bytes.push byte + @waiting_bytes -= 1 + if @waiting_bytes == 0 + dispatch_command + end end end - # Sets the state of simulated Roomba to moving - def move(*args) - # update x, y; check if any obstacle coordinates fall inside roomba's radius; - # queue sensor readings in some array to simulate TX/RX - @velocity = signed_integer([args[0], args[1]]) - @moving = (@velocity.abs > 0) ? true : false - if @moving - puts "Moving at #{@velocity}mm/s" + def calculate_rotation(step_time) + if @turning + calculate_spin_degree(@velocity, step_time) else - puts "Stopped moving" + 0 end - @timestamp = Time.now - @degree = signed_integer([args[2], args[3]]) - @turning = (@degree.abs == 1) ? true : false - # Simulation can currently only handle the following (cannot support half-points or curves) - if @facing > 45 && @facing < 135 - @facing = 90 - elsif @facing >= 135 && @facing < 225 - @facing = 180 - elsif @facing >= 225 && @facing < 315 - @facing = 270 + end + + def calculate_distance(step_time) + if @turning + 0 else - @facing = 0 + @velocity * step_time end - return true end - def moving? - return @moving + + # When the RoombaSimulation requests a byte from + # the simulated serial port, it gets shifted off the array of available bytes. + def getbyte + @readings.shift end - def radius - RADIUS + def read_timeout=(timeout) end - def born_in(world) - @world = world + + private + + def dispatch_command + command = @command_bytes.shift + case command + when 137 + setup_move(@command_bytes) + when 149 + prepare_readings(@command_bytes) + when 128,130 + LOGGER.info "Roomba API ready to receive commands" + else + LOGGER.debug "Command not implemented #{command}" + end + end + + def bytes_needed_per_command(command) + bytes_per_command = { 137 => 4, 128 => 0, 130 => 0, 149 => 3 } + bytes_per_command[command] || 0 end + + # Sets the state of simulated Roomba to moving + def setup_move(args) + # update x, y; check if any obstacle coordinates fall inside roomba's radius; + # queue sensor readings in some array to simulate TX/RX + @velocity = signed_integer([args[0], args[1]]) + moving = (@velocity.abs > 0) ? true : false + if moving + LOGGER.debug "Moving at #{@velocity}mm/s" + else + LOGGER.debug "Stopped moving" + end + @degree = signed_integer([args[2], args[3]]) + @turning = (@degree.abs == 1) ? true : false - def render - ui = {} - ui['x'] = x - ui['y'] = y - ui['radius'] = radius - ui['name'] = 'Roomba' - ui + return true end def prepare_readings(*args) - args.each do |request| - SENSORS.each do |sensor| + args.first.each do |request| + Roomba::SENSORS.each do |sensor| if sensor[1][:packet] == request - if respond_to? "prepare_reading_#{request}".to_sym + begin send("prepare_reading_#{request}".to_sym) - else + rescue NameError 1.upto(sensor[1][:bytes]) { @readings.push(0) } end end @@ -105,78 +124,12 @@ def prepare_readings(*args) # Roomba's state, so that other sensors that also check Roomba's immediate # environment can leverage the same current X,Y coordinates def prepare_reading_7 - start_x = @x - start_y = @y - previous_x = 0 - previous_y = 0 - reading = 0 #value of bump_and_drops sensors - latest_check_time = Time.now - difference = latest_check_time - @timestamp #difference from last reading - puts @turning.inspect - if !@turning - puts "Time diff: #{difference}" - distance = (@velocity * difference).to_i - puts "Travelled #{distance}mm" - if distance > 0 #driving forward - 1.upto(distance) do |x| - previous_x = @x - previous_y = @y - case @facing - when 0 - @y = @y+1 - when 90 - @x = @x+1 - when 180 - @y = @y-1 - when 270 - @x = @x-1 - end - puts "N:#{@facing}, X:#{@x} Y:#{@y}" - reading = (@world.collision_with?(self)) ? 1 : 0 - break if reading == 1 - end - else #driving backward, driving blind (no sensors!) - distance.upto(0) do |x| - previous_x = @x - previous_y = @y - case @facing - when 0 - @y = @y-1 - when 90 - @x = @x-1 - when 180 - @y = @y+1 - when 270 - @x = @x+1 - end - puts "N:#{@facing}, X:#{@x} Y:#{@y}" - blind_reading = (@world.collision_with?(self)) ? 1 : 0 - break if blind_reading == 1 - end - reading = 0 #always return 0 when driving blind - end - if reading == 1 #hit something at that coordinate, impassable, back to previous coordinate (don't share points) - @x = previous_x - @y = previous_y - end - if start_x != @x || start_y != @y #some coordinate changed - @timestamp = latest_check_time #enough time accumalated to register movement, record this check in timestamp so we don't accelerate exponentially - end + # TODO: distinguish collisions with bumpers and not bumpers + if @bumpers && @bumpers.any?(&:got_collisions?) + @readings.push 1 else - @timestamp = latest_check_time - @facing = @facing + calculate_spin_degree(@velocity, latest_check_time) - puts "N:#{@facing}" + @readings.push 0 end - - @readings.push(reading) end - # When the RoombaSimulation requests a byte from - # the simulated serial port, it gets shifted off the array of available bytes. - def getbyte - readings.shift - end - - def read_timeout=(timeout) - end end diff --git a/lib/roomba_simulation.rb b/lib/roomba_simulation.rb index 5a2ccba..ce9f7db 100644 --- a/lib/roomba_simulation.rb +++ b/lib/roomba_simulation.rb @@ -1,19 +1,31 @@ -class RoombaSimulation < Roomba - def initialize(port="simulation", latency=0, baud=115200) - @serial = RoombaSerialSimulation.new - super(port, latency, baud, @serial) +# ######## +# This class is an specific implementation of a simulation for roomba +# ####### +class RoombaSimulation < RobotSimulation + def initialize(simulation) + world = simulation.world + bumpers = [ Bumper.new(self, -45, 30, world), Bumper.new(self, 45, 30, world) ] + serial = RoombaSerialSimulation.new(bumpers) + modify_roomba_internals(simulation) + real_robot = Roomba.new('simulation', 0, 115200, serial) + world.spawn(self) + + super(world, serial, real_robot) self end - def render - @serial.render + def radius + Roomba::Specification::RADIUS end - def born_in(*args) - @serial.born_in(*args) - end + private - def write(*args) - @serial.write(*args) + #TODO: TOTALLY Hacky. Eventually Roomba can get its time from an API so we can hook there. + def modify_roomba_internals(simulation) + Roomba.send(:define_method, :current_time) do + simulation.current_time + end end end + + diff --git a/lib/simulator/bumper.rb b/lib/simulator/bumper.rb new file mode 100644 index 0000000..58021d4 --- /dev/null +++ b/lib/simulator/bumper.rb @@ -0,0 +1,32 @@ +###################################### +# This class represent a bumper sensor (it returns collisions when +# touching something) +##################################### +class Bumper + # The robot we belong to, the angle where the bumper is installed + # in case the bumper has an extension, lenght of it + # and the world we belong to + def initialize(robot, angle, lenght, world) + @robot, @angle, @lenght, @world = robot, angle, lenght, world + end + + def got_collisions? + positions_to_test.any?{ |pos| @world.collision_with?(pos) } + end + + private + def positions_to_test + [bumper_position(@angle-lenght), bumper_position(@angle), bumper_position(@angle+lenght)] + end + + def bumper_position(angle) + bumper_x = @robot.pos.x + @robot.radius * Math.cos(world_angle(angle)) + bumper_y = @robot.pos.y + @robot.radius * Math.sin(world_angle(angle)) + Position.new(bumper_x, bumper_y) + end + + def world_angle(angle) + Angle.degrees_to_radians(@robot.pose.angle + angle) + end + +end diff --git a/lib/simulator/robot_simulation.rb b/lib/simulator/robot_simulation.rb new file mode 100644 index 0000000..7f66f73 --- /dev/null +++ b/lib/simulator/robot_simulation.rb @@ -0,0 +1,67 @@ +# ##################### +# This class is an abstract interface for simulated robots +# it can not be used directly +# Child classes need to call the initializer with a driver and the +# class which drives the robot. +# They need also to implement radius so we know its geometry +# ################### +class RobotSimulation + attr_reader :pose + + def initialize(world, driver, real_robot) + raise "A virtual robot needs virtual hardware" if driver.nil? + raise "A virtual robot needs a real robot implementation" if real_robot.nil? + raise "A virtual robot needs a simulation" if world.nil? + @driver = driver + @real_robot = real_robot + @world = world + + # Set defaults if not set in the initializer block + # These defaults match the previously hard-coded values + @pose = Ein::Pose.new(Ein::Position.new(0,0),0) + + yield self if block_given? + end + + def radius + 100 #default radius, this method should be overloaded by every robot + end + + #this is needed so the physics simulation treat objects of this class as circles + def physical_shape + :circle + end + + def step(step_time) + @previous_pose = @pose.dup + @pose = @pose.advance(@driver.calculate_distance(step_time), @driver.calculate_rotation(step_time)) + LOGGER.debug "#@pose" + end + + #TODO: this is fugly, should be a better way to stop on obstacles + def step_back + @pose = @previous_pose.dup + end + + def render + ui = {} + ui['x'] = @pose.position.x + ui['y'] = @pose.position.y + ui['angle'] = @pose.angle + ui['radius'] = radius + ui['name'] = 'Roomba' + ui + end + + def pos + @pose.position.round + end + + private + + def method_missing(method, *args) + #we will raise if the method is not there either + return @real_robot.send(method, *args) + end + +end diff --git a/lib/simulator/simulator.rb b/lib/simulator/simulator.rb new file mode 100644 index 0000000..4161e05 --- /dev/null +++ b/lib/simulator/simulator.rb @@ -0,0 +1,16 @@ +############################## +# +#This class is the highest level entity, controls how +#the simulation behaves, contains the world and the robots +# +############################### +class Simulator < Ein::Simulator + +end + +##### +# Our logger, default level is info. +# Please use only .info and .debug levels +##### +LOGGER = Logger.new(STDOUT) +LOGGER.level = Logger::INFO diff --git a/lib/world.rb b/lib/world.rb deleted file mode 100644 index 7c47d0d..0000000 --- a/lib/world.rb +++ /dev/null @@ -1,63 +0,0 @@ -############################## -# -# This class represents the virtual world our simulation runs in -# -############################## -class World - - def initialize(*roombots) - @robots = [] - read_world - roombots.each do |bot| - spawn(bot) - end - self - end - - def render - world_ui = {} - world_ui['boundaries'] = @boundaries - world_ui['obstacles'] = @obstacles - @robots.each do |r| - world_ui['robot'] = r.render - end - world_ui.to_json - end - - def spawn(robot) - @robots.push(robot) - robot.born_in self - end - - def robot(index=0) - @robots[index] - end - - - #TODO: collision with a serial it is not very intuitive - def collision_with?(serial) - x = serial.x - y = serial.y - radius = serial.radius - - if (@boundaries[0] - x).abs == radius || (@boundaries[1] - x).abs == radius || (@boundaries[2] - y).abs == radius || (@boundaries[3] - y).abs == radius - return true - end - @obstacles.each do |z| - distance = Math.sqrt((x - z[:x])**2 + (y - z[:y])**2) # Pythagoras, miss you buddy. RIP - return true if distance <= (z[:radius] + radius) - end - false - end - - private - def read_world - #TODO: read from external .yml or something - #TODO: mass and shape for the obstacles - @boundaries ||= [1000, -1000, 800, -800]#x,-x, y, -y - @obstacles ||= [{x:0, y:500, radius:20}, {x:300, y:0, radius:20},{x:-900, y:-700, radius:10}] - - end - -end - diff --git a/test/test_helper.rb b/test/test_helper.rb index 275cddd..43e5daa 100644 --- a/test/test_helper.rb +++ b/test/test_helper.rb @@ -10,7 +10,7 @@ # :pretty - new pretty reporter # :marshal - dump output as YAML (normal run mode only) # :cue - interactive testing - c.format = :outline + c.format = :pretty # turn on invoke/execute tracing, enable full backtrace c.trace = true # use humanized test names (works only with :outline format) diff --git a/test/unit/simulation_test.rb b/test/unit/simulation_test.rb index 1c5eebd..687b5c6 100644 --- a/test/unit/simulation_test.rb +++ b/test/unit/simulation_test.rb @@ -1,7 +1,43 @@ + require 'test_helper' -class SimulationTest < ActiveSupport::TestCase - # test "the truth" do - # assert true - # end +describe Simulation do + before(:each) do + @simulation = Simulator.new + @roo = RoombaSimulation.new + @simulation.add_robot(@roo) + end + + it "can be started and stopped" do + @simulation.start + @simulation.running?.must_equal true + @simulation.stop + @simulation.running?.must_equal false + end + + + it "should not move the robot if the simulation has not started" do + @roo.move(50) + sleep(0.1) + @roo.x.must_equal 0 + @roo.y.must_equal 0 + end + + + it "must spawn the robot into 0,0" do + @simulation.start + @roo.x.must_equal 0 + @roo.y.must_equal 0 + @simulation.stop + end + + it "should move in a straight line" do + @simulation.start + @roo.move(50) + sleep 0.3 + @roo.x.must_equal 0 + @roo.y.must_equal 50 + end + + end diff --git a/test/unit/world_test.rb b/test/unit/world_test.rb deleted file mode 100644 index da7d798..0000000 --- a/test/unit/world_test.rb +++ /dev/null @@ -1,36 +0,0 @@ -require 'test_helper' - -# TO RUN: -# ruby -Itest test/unit/world_test.rb - -describe World do - - describe 'basic initialization' do - - let(:world) do - World.new(RoombaSimulation.new) - end - - it "creates an instance" do - world.must_be_instance_of World - end - - it "should create a default robot" do - world.robot.must_be_instance_of RoombaSimulation - end - - it "should not create more than 1 initial robot" do - world.robot(1).must_equal nil - end - - it "should render the boundaries" do - world.render.include?("boundaries").must_equal true - end - - it "should render the obstacles" do - world.render.include?("obstacles").must_equal true - end - end - -end -