IR sensor using I2C device - Robot control with IR detection


1. Download and open C# project file
  1. Download source code.

    IR.Sample2.zip



2. C# source code for IR.Sample2
  1. Program.cs

      
    
    using System;
    
    using Microsoft.SPOT;
    using Microsoft.SPOT.Input;
    using Microsoft.SPOT.Presentation;
    using Microsoft.SPOT.Presentation.Controls;
    using Microsoft.SPOT.Hardware;
    
    using SPL.Script.Engine;
    
    namespace SPLMFConsoleWindow1
    {
        public class Program : Microsoft.SPOT.Application
        {
            public static void Main()
            {
                Program myApplication = new Program();
    
                Window mainWindow = myApplication.CreateWindow();
    
                GPIOButtonInputProvider inputProvider = new GPIOButtonInputProvider(null);
    
                myApplication.Run(mainWindow);
            }
    
            private Window mainWindow;
    
            //##########################  Sample Code  #################################
            private ConsoleWindow _consoleWindow;
    
            private I2CDevice _device = null;
            private I2CDevice _i2c2 = null;
            //##########################################################################
    
            public Window CreateWindow()
            {
                mainWindow = new Window();
                mainWindow.Height = SystemMetrics.ScreenHeight;
                mainWindow.Width = SystemMetrics.ScreenWidth;
    
                //##########################  Sample Code  #################################
                Font font = Resources.GetFont(Resources.FontResources.small);
                _consoleWindow = new ConsoleWindow(font);
    
                mainWindow.Child = _consoleWindow;
                //##########################################################################
    
                mainWindow.AddHandler(Buttons.ButtonUpEvent, new ButtonEventHandler(OnButtonUp), false);
    
                mainWindow.Visibility = Visibility.Visible;
    
                Buttons.Focus(mainWindow);
    
                //##########################  Sample Code  #################################
                _consoleWindow.WriteLine("Push Select-Button to make a robot go forwards.");
                //##########################################################################
    
    
                //##########################  Sample Code  #################################
                I2CDevice.Configuration config = new I2CDevice.Configuration(
                                58,
                                100
                                );
    
                _device = new I2CDevice(config);
    
    
                I2CDevice.Configuration i2c2_config = new I2CDevice.Configuration(
                                60,
                                100
                                );
    
                _i2c2 = new I2CDevice(i2c2_config);
    
                System.Threading.Thread serialReaderThread = new System.Threading.Thread(i2c2_Reader);
                serialReaderThread.Start();
    
                //##########################################################################
    
                return mainWindow;
            }
    
            private void OnButtonUp(object sender, ButtonEventArgs e)
            {
                //##########################  Sample Code  #################################
                if (e.Button == Microsoft.SPOT.Hardware.Button.VK_SELECT)
                {
                    byte[] wBuff = new byte[3];
                    wBuff[0] = 4;
                    wBuff[1] = 120;     //120-100
                    wBuff[2] = 120;
    
                    I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                    I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };
    
                    //Go Forwards
                    int count = _device.Execute(wTrArr, 1000);
    
                    _consoleWindow.WriteLine("Go forwards");
                }
                //##########################################################################
            }
    
    
            //##########################  Sample Code  #################################
            private void i2c2_Reader()
            {
                byte[] rBuff = new byte[2];
    
                while (true)
                {
                    try
                    {
                        I2CDevice.I2CReadTransaction rTr = _device.CreateReadTransaction(rBuff);
                        I2CDevice.I2CTransaction[] rTrArr = new I2CDevice.I2CTransaction[] { rTr };
    
                        int count = _i2c2.Execute(rTrArr, 1000);
    
                        if (rBuff != null)
                        {
                            ushort distance = Util.BytesToUShort(rBuff);
    
                            if (distance > 0 && distance < 1000)
                            {
                                byte[] wBuff = new byte[3];
                                wBuff[0] = 2;
                                wBuff[1] = 190;     //190-100
                                wBuff[2] = 120;     //0.2
    
                                I2CDevice.I2CWriteTransaction wTr = _device.CreateWriteTransaction(wBuff);
                                I2CDevice.I2CTransaction[] wTrArr = new I2CDevice.I2CTransaction[] { wTr };
    
                                //Turn 90
                                count = _device.Execute(wTrArr, 1000);
    
                                System.Threading.Thread.Sleep(500);
    
                                //Turn 90
                                count = _device.Execute(wTrArr, 1000);
    
                                System.Threading.Thread.Sleep(500);
    
                                wBuff[0] = 4;
                                wBuff[1] = 120;     //0.2
                                wBuff[2] = 120;     //0.2
                                count = _device.Execute(wTrArr, 500);
                            }
                        }
                    }
                    catch { }
    
                    System.Threading.Thread.Sleep(300);
                }
            }
    
            private void LogInfo(string log)
            {
                this.Dispatcher.BeginInvoke(
                    new DispatcherOperationCallback(
                        delegate(object report)
                        {
                            _consoleWindow.WriteLine(log);
                            return null;
                        }
                        ), null
                    );
            }
            //##########################################################################
        }
    }
    
    
      



3. Execute C# project
  1. Open "Properties" page of C# project.

  2. Choose ".NET Micro Framework" tab.

  3. Select "SPLMFConsoleEmulator" from the emulator list.



  4. Press "F5" key to run sample code.




4. Launch simulation environment
  1. Start "SPL script editor for MSRDS" and add SPL simulation script.

  2. Add script on the editor as follows.



      
    MFEmulator    emul1
    	/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 
    
      


  3. Save script.

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



5. Start demo
  1. Click Select-Button to start demo.



  2. You can see that the robot repeat go forwards and turn back when it faces the obstacle.