[Go to list] 
 
Maze Explorer Mission using I2C and SerialPort


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



       
    MFEmulator    emul1
	/TargetEmulator:SPLMFConsoleEmulator
	/Procedure_I2CWrite:proc_i2c_write

StartSimulationEngine  
	/FileName:"SimState/mazestate1.xml"

AddDifferentialDriveEntity    base1
	/Position:0    0    4.3
	/Orientation:0    90    0

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

AddFollowingChaseCamera    cam1
	/Position:0  0.5  2.0
	/ChaseTarget:base1

FlushScript  

sendBytes = Util.CreateArrayByte(361)

skip_count = 5

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 = 5
	}
	
	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. SPL MF script for Maze Mission
  1. Choose "Application" tab from the "SPL MF Console Emulator".

  2. Add below script on the editor box of "Application" tab.



       
    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	
	skip_count_for_keep_going = 0
	wait_count_for_turn_left = 0
	
	while(true)
	{	
		COM3.Read(lrf_buff, 0, 361)
				
		d1 = lrf_buff[355] * 32
		d2 = lrf_buff[270] * 32
		
		d3 = Math.Min(lrf_buff[150], lrf_buff[180])
		d3 = Math.Min(d3, lrf_buff[210])
		d3 = d3 * 32

		if (skip_count_for_keep_going == 0)
		{
			if (d1 < 100 || d2 < 300 || d3 < 1000)
			{
				buff1[0] = 2
				buff1[1] = 10	//-90   10 - 100
				buff1[2] = 120	//0.2
				i2c1.Write(buff1)

				skip_count_for_keep_going = 3
			}
			else
			{
				if (d1 > 2500 && wait_count_for_turn_left == 0 && skip_count == 0)
				{					
					wait_count_for_turn_left = 6
				}	
			}
		}
						
		if (skip_count_for_keep_going == 1)
		{
			buff1[0] = 4
			buff1[1] = 110	//0.1
			buff1[2] = 110	//0.1
			i2c1.Write(buff1)			
		}

		if (wait_count_for_turn_left == 1)
		{
			//left turn
			buff1[0] = 2
			buff1[1] = 190	//90   190-100
			buff1[2] = 120	//0.2
			i2c1.Write(buff1)
			
			skip_count_for_keep_going = 3
			skip_count = 4
		}

		if (skip_count > 0)
			skip_count--
						
		if (skip_count_for_keep_going > 0)
			skip_count_for_keep_going--
			
		if (wait_count_for_turn_left > 0)
			wait_count_for_turn_left--					
			
	}
	
end
    		  


  3. Click "Send to MF Application" button

  4. Wait two or three seconds after click "Send to MF Application" button.

  5. You can see a mobile robot move itself through maze.





[Go to list]