[Go to list] 
 
IR sensor using I2C device - Robot control with IR 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

AddInfraredRangeEntity    ir1
	/Position:0  0.4  -0.3
	/ParentEntity:base1
	/Procedure_SensorNotify:proc_ir

FlushScript  

procedure  proc_ir
	
	distance = value.RawMeasurement	// 0.0 ~ 1.0 m
	distance = distance * 1000	//convert to 0 ~ 1000 cm unit
	
	print "IR Distance: " + distance
	
	//convert type as ushort
	distance = Util.ToUShort(distance)	
	
	//convert ushort to byte[2]
	sendBytes = Util.UShortToBytes(distance)
	
	emul1.SetI2CReadBuffer("i2c2", sendBytes)
	
end 

procedure  proc_i2c_write
	
	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 I2C device named "i2c2" and reaction logic for the IR 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

//New I2C device for detection of IR sensor
I2CDevice    i2c2
	/Address:60
	/ClockRateKhz:100


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_ir with concur

procedure proc_read_ir

	skip_count = 0

	while(true)
	{	
		rBuff = Util.CreateArrayByte(2)
		
		i2c2.Read(rBuff)
	
		distance = Util.BytesToUShort(rBuff)
		
		if (distance > 0 && distance < 1000 && skip_count == 0)
		{
			//turn 90 degrees
			buff1[0] = 2
			buff1[1] = 190	//90 degrees
			buff1[2] = 130	//0.3
			i2c1.Write(buff1)
			
			wait 1000
			
			//turn 90 degrees
			buff1[0] = 2
			buff1[1] = 190	//90 degrees
			buff1[2] = 130	//0.3
			i2c1.Write(buff1)			
			
			wait 1000
			
			skip_count = 1
		}
		
		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--
			
		wait 300
	}
	
end
    		  


  3. Click "Send to MF Application" button

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



[Go to list]