[Go to list] 
 
LRF (Laser Range Finder) sensor using SerialPort - Add LRF sensor on the simulated mobile robot


1. Start "SPL script editor for MSRDS" and add SPL simulation script
  1. Add script on the MSRDS SPL editor as follows.

       
    MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicenv4.xml"

AddDifferentialDriveEntity    base1

FlushScript
    		  



2. Add LRF sensor as follows.
  1. Add script on the editor as follows.



       
    MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicenv4.xml"

AddDifferentialDriveEntity    base1

AddLaserRangeFinderEntity    lrf1
	/Position:0  0.4  0
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_lrf

FlushScript
    		  



3. Add procedure definition named "proc_lrf" for LRF sensor event.
  1. Add script on the editor as follows.



       
    MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicenv4.xml"

AddDifferentialDriveEntity    base1

AddLaserRangeFinderEntity    lrf1
	/Position:0  0.4  0
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_lrf

FlushScript  

sendBytes = Util.CreateArrayByte(361)

skip_count = 4

ready_flag = false

procedure  proc_lrf
	
	if (skip_count <= 0 && ready_flag)
	{
		lrfbuff = value.DistanceMeasurements
	
		for (i = 0; i < 361; i++)
		{
			//convert scalse to byte 8000 -> 250
			distance = lrfbuff[i] / 32
			sendBytes[i] = Util.ToByte(distance)
		}
		
		emul1.SendToSerialPort("COM3", sendBytes)
		
		d180 = lrfbuff[180]
		print d180
		
		skip_count = 4
	}
	
	skip_count--
end
    		  



4. Add procedure definition named "proc_i2c_write" for MF emulator event.
  1. Add script on the editor as follows.



       
    MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  "SimState/basicenv4.xml"

AddDifferentialDriveEntity    base1

AddLaserRangeFinderEntity    lrf1
	/Position:0  0.4  0
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_lrf

FlushScript  

sendBytes = Util.CreateArrayByte(361)

skip_count = 4

ready_flag = false

procedure  proc_lrf
	
	if (skip_count <= 0 && ready_flag)
	{
		lrfbuff = value.DistanceMeasurements
	
		for (i = 0; i < 361; i++)
		{
			//convert scalse to byte 8000 -> 250
			distance = lrfbuff[i] / 32
			sendBytes[i] = Util.ToByte(distance)
		}
		
		emul1.SendToSerialPort("COM3", sendBytes)
		
		d180 = lrfbuff[180]
		print d180
		
		skip_count = 4
	}
	
	skip_count--
end 

procedure  proc_i2c_write
	
	ready_flag = true

	if (value.Id == "i2c1")
	{
		command_type = value.ReceivedBytes[0]
		
		if (command_type == 1)
		{		
			distance = value.ReceivedBytes[1]
			power = value.ReceivedBytes[2]
	
			power = power - 100

			distance = Util.ToDouble(distance / 10.0)
			power = Util.ToDouble(power / 100.0)

			print "Distance -> " + distance.ToString() + " m / Power -> " + power.ToString()
			
			base1.GoTo(distance, power)
		}
		else if (command_type == 2)
		{		
			degrees = value.ReceivedBytes[1]
			power = value.ReceivedBytes[2]
	
			degrees = degrees - 100
			power = power - 100

			degrees = Util.ToDouble(degrees)
			power = Util.ToDouble(power / 100.0)

			print "Degrees -> " + degrees.ToString() + " / Power -> " + power.ToString()
			
			base1.Turn(degrees, power)
		}
		else if (command_type == 4)
		{		
			left_power = value.ReceivedBytes[1]
			right_power = value.ReceivedBytes[2]
	
			left_power = left_power - 100
			right_power = right_power - 100

			left_power = Util.ToDouble(left_power / 100.0)
			right_power = Util.ToDouble(right_power / 100.0)

			print "Left -> " + left_power.ToString() + " / Right -> " + right_power.ToString()
			
			base1.Go(left_power, right_power)
		}
	}

end
    		  


  2. Save script.

  3. Press "F5" key or click "Run" icon.

  4. Added LRF sensor will be shown as follows.

[Go to list]