Commit 4572409b authored by lildholdt's avatar lildholdt
Browse files

Partical swarm - working with sensor readings

parent 714036db
......@@ -7,7 +7,7 @@ public class PilotRoute
private SlaveIOStreams PC;
private double distFactor = 0.99;
private double wheelDiameterL = 5.595*distFactor, wheelDiameterR = 5.6*distFactor, trackWidth = 16.27;
private double travelSpeed = 20, rotateSpeed = 45;
private double travelSpeed = 5, rotateSpeed = 45;
private NXTRegulatedMotor left = Motor.B;
private NXTRegulatedMotor right = Motor.C;
private LightSensor lightsensor = new LightSensor(SensorPort.S4);
......@@ -26,7 +26,7 @@ public class PilotRoute
private void sendMove(Move move)
{
PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
PC.output(move.getDistanceTraveled());
PC.output(move.getDistanceTraveled() * 10);
PC.output(move.getAngleTurned());
}
......@@ -53,25 +53,25 @@ public class PilotRoute
while ( ! Button.ENTER.isDown()) Thread.yield();
Sound.twoBeeps();
for(int i=0;i<30;i++)
for(int i=0;i<15;i++)
{
travel(5);
sendLightValue(400);
travel(1);
sendLightValue(lightsensor.getLightValue());
}
for(int i=0;i<10;i++)
for(int i=0;i<5;i++)
{
travel(5);
sendLightValue(600);
travel(1);
sendLightValue(lightsensor.getLightValue());
}
for(int i=0;i<10;i++)
for(int i=0;i<5;i++)
{
travel(5);
sendLightValue(400);
travel(1);
sendLightValue(lightsensor.getLightValue());
}
for(int i=0;i<10;i++)
for(int i=0;i<5;i++)
{
travel(5);
sendLightValue(600);
travel(1);
sendLightValue(lightsensor.getLightValue());
}
while ( ! Button.ENTER.isDown()) Thread.yield();
......
......@@ -25,7 +25,7 @@ public class Particle
private static Random rand = new Random();
private Pose pose;
private float weight = 1;
private int blackWhiteThreshold = 500;
private int blackWhiteThreshold = 42;
/**
* Create a particle with a specific pose
......@@ -76,7 +76,7 @@ public class Particle
{
if ( m.getColor(pose) == Color.BLACK )
{
if ( lightValue > blackWhiteThreshold)
if ( lightValue < blackWhiteThreshold)
weight = 0.9f;
else
weight = 0.1f;
......@@ -85,7 +85,7 @@ public class Particle
{
if ( m.getColor(pose) == Color.WHITE )
{
if ( lightValue > blackWhiteThreshold)
if ( lightValue < blackWhiteThreshold)
weight = 0.1f;
else
weight = 0.9f;
......
......@@ -92,12 +92,11 @@ public class RobotMonitor
public void goSimulation()
{
for ( int i=0; i < 60; i++)
for ( int i=0; i < 30; i++)
{
motionUpdate(getMove());
sensorUpdate(getSensorValue());
}
}
public static void main (String [] args)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment