| |
Color sensor using I2C device - Add Color sensor on the simulated mobile robot
1. Start "SPL script editor for MSRDS" and add SPL simulation script
- Add script on the MSRDS SPL editor as follows.
| |
MFEmulator emul1
/TargetEmulator:SPLMFConsoleEmulator
/Procedure_I2CWrite:proc_i2c_write
StartSimulationEngine "SimState/basicsim.xml"
AddNewEntity ent1 /Position:1.5 0.3 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:1 0 0 1
AddNewEntity ent2 /Position:0 0.4 -1.5
AddBoxShape /Dimensions:2.0 0.8 0.6 /Mass:100
/DiffuseColor:1 1 1 1
AddNewEntity ent3 /Position:-1.5 0.4 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:0 0 0.627451 1
AddDifferentialDriveEntity base1
FlushScript
| |
|
2. Add Color sensor as follows.
- Add script on the editor as follows.
| |
MFEmulator emul1
/TargetEmulator:SPLMFConsoleEmulator
/Procedure_I2CWrite:proc_i2c_write
StartSimulationEngine "SimState/basicsim.xml"
AddNewEntity ent1 /Position:1.5 0.3 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:1 0 0 1
AddNewEntity ent2 /Position:0 0.4 -1.5
AddBoxShape /Dimensions:2.0 0.8 0.6 /Mass:100
/DiffuseColor:1 1 1 1
AddNewEntity ent3 /Position:-1.5 0.4 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:0 0 0.627451 1
AddDifferentialDriveEntity base1
AddColorSensorEntity color1
/Position:0 0.4 -0.3
/ParentEntity:base1
/Procedure_SensorNotify:proc_color
FlushScript
| |
|
3. Add procedure definition named "proc_color" for color sensor event.
- Add script on the editor as follows.
|
| |
MFEmulator emul1
/TargetEmulator:SPLMFConsoleEmulator
/Procedure_I2CWrite:proc_i2c_write
StartSimulationEngine "SimState/basicsim.xml"
AddNewEntity ent1 /Position:1.5 0.3 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:1 0 0 1
AddNewEntity ent2 /Position:0 0.4 -1.5
AddBoxShape /Dimensions:2.0 0.8 0.6 /Mass:100
/DiffuseColor:1 1 1 1
AddNewEntity ent3 /Position:-1.5 0.4 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:0 0 0.627451 1
AddDifferentialDriveEntity base1
AddColorSensorEntity color1
/Position:0 0.4 -0.3
/ParentEntity:base1
/Procedure_SensorNotify:proc_color
FlushScript
procedure proc_color
r = value.NormalizedAverageRed //0.0 ~ 1.0
g = value.NormalizedAverageGreen //0.0 ~ 1.0
b = value.NormalizedAverageBlue //0.0 ~ 1.0
r = r * 255 //convert to 0 ~ 255
g = g * 255 //convert to 0 ~ 255
b = b * 255 //convert to 0 ~ 255
//convert type as byte
sendBytes = Util.CreateArrayByte(3)
sendBytes[0] = Util.ToByte(r)
sendBytes[1] = Util.ToByte(g)
sendBytes[2] = Util.ToByte(b)
emul1.SetI2CReadBuffer("i2c2", sendBytes)
end
| |
|
4. Add procedure definition named "proc_i2c_write" for MF emulator event.
- Add script on the editor as follows.
|
| |
MFEmulator emul1
/TargetEmulator:SPLMFConsoleEmulator
/Procedure_I2CWrite:proc_i2c_write
StartSimulationEngine "SimState/basicsim.xml"
AddNewEntity ent1 /Position:1.5 0.3 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:1 0 0 1
AddNewEntity ent2 /Position:0 0.4 -1.5
AddBoxShape /Dimensions:2.0 0.8 0.6 /Mass:100
/DiffuseColor:1 1 1 1
AddNewEntity ent3 /Position:-1.5 0.4 0
AddBoxShape /Dimensions:0.6 0.8 2.0 /Mass:100
/DiffuseColor:0 0 0.627451 1
AddDifferentialDriveEntity base1
AddColorSensorEntity color1
/Position:0 0.4 -0.3
/ParentEntity:base1
/Procedure_SensorNotify:proc_color
FlushScript
procedure proc_color
r = value.NormalizedAverageRed //0.0 ~ 1.0
g = value.NormalizedAverageGreen //0.0 ~ 1.0
b = value.NormalizedAverageBlue //0.0 ~ 1.0
r = r * 255 //convert to 0 ~ 255
g = g * 255 //convert to 0 ~ 255
b = b * 255 //convert to 0 ~ 255
//convert type as byte
sendBytes = Util.CreateArrayByte(3)
sendBytes[0] = Util.ToByte(r)
sendBytes[1] = Util.ToByte(g)
sendBytes[2] = Util.ToByte(b)
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
| |
|
- Save script.
- Press "F5" key or click "Run" icon.
- Added Color sensor will be shown as follows.
|
