[Go to list] 
 
LRF (Laser Range Finder) sensor using SerialPort - Robot control with LRF detection


1. Start "SPL script editor for MSRDS" and add SPL simulation script
  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.



2. Add a new serial port named "COM3" and reaction logic for the LRF sensor event
  1. Clear editor box of MF application.

  2. Add below script on the editor box of MF application.



       
    I2CDevice    i2c1
	/Address:58
	/ClockRateKhz:100

SerialPort    COM3
	/PortName:COM3
	/BaudRate:9600

lrf_buff = Util.CreateArrayByte(361)

buff1 = Util.CreateArrayByte(3)

//go forwards
buff1[0] = 4
buff1[1] = 120	//0.2
buff1[2] = 120	//0.2

//make a robot move itselft
i2c1.Write(buff1)

//call procedure with the separate thread
call proc_read_lrf with concur


procedure proc_read_lrf

	skip_count = 0

	while(true)
	{	
		COM3.Read(lrf_buff, 0, 361)
		
		//distance of front
		distance_180 = lrf_buff[180] * 32
		
		if (distance_180 > 0 && distance_180 < 1500 && skip_count == 0)
		{
			//turn 90 degrees
			buff1[0] = 2
			buff1[1] = 190	//90 degrees
			buff1[2] = 130	//0.3
			i2c1.Write(buff1)
			
			skip_count = 5
		}

		if (skip_count == 3)
		{
			//turn 90 degrees
			buff1[0] = 2
			buff1[1] = 190	//90 degrees
			buff1[2] = 130	//0.3
			i2c1.Write(buff1)
		}
		
		if (skip_count == 1)
		{
			//go forwards
			buff1[0] = 4
			buff1[1] = 120	//0.2
			buff1[2] = 120	//0.2
			i2c1.Write(buff1)
		}
		
		if (skip_count > 0)
			skip_count--			
	}
	
end
    		  


  3. Click "Send to MF Application" button

  4. You can see a mobile robot turn reverse when the distance of LRF sensor is under 1.5 m.



[Go to list]