Commit 3ffad4cb authored by lildholdt's avatar lildholdt
Browse files

Working simulation - partical swarm

parent ddd9f7a6
......@@ -28,7 +28,11 @@ public class PilotRoute
PC.output((move.getMoveType() == Move.MoveType.TRAVEL? 0:1 ));
PC.output(move.getDistanceTraveled());
PC.output(move.getAngleTurned());
PC.output(lightsensor.getLightValue());
}
private void sendLightValue(int value)
{
PC.output(value);
}
private void travel(double distance)
......@@ -49,9 +53,25 @@ public class PilotRoute
while ( ! Button.ENTER.isDown()) Thread.yield();
Sound.twoBeeps();
for(int i=0;i<40;i++)
for(int i=0;i<30;i++)
{
travel(5);
sendLightValue(400);
}
for(int i=0;i<10;i++)
{
travel(5);
sendLightValue(600);
}
for(int i=0;i<10;i++)
{
travel(5);
sendLightValue(400);
}
for(int i=0;i<10;i++)
{
travel(5);
sendLightValue(600);
}
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,43 +92,12 @@ public class RobotMonitor
public void goSimulation()
{
for ( int i=0; i < 40; i++)
for ( int i=0; i < 60; i++)
{
motionUpdate(getMove());
sensorUpdate(getSensorValue());
}
/*
for ( int i=0; i < 16; i++)
{
motionUpdate(getMove());
sensorUpdate(400);
}
for ( int i=0; i < 5; i++)
{
move = new Move(Move.MoveType.TRAVEL, 10, 0, false);
motionUpdate(move);
sensorUpdate(600);
}
move = new Move(Move.MoveType.TRAVEL, 100, 0, false);
motionUpdate(move);
sensorUpdate(600);
move = new Move(Move.MoveType.TRAVEL, 10, 0, false);
motionUpdate(move);
sensorUpdate(400);
move = new Move(Move.MoveType.TRAVEL, 10, 0, false);
motionUpdate(move);
sensorUpdate(400);
Pose p = particles.getPose();
System.out.println("Position "+p.getX()+" "+p.getY()+" "+p.getHeading());
System.out.println("Accuracy "+ particles.getSigmaX()+ " "+ particles.getSigmaY()+" "+particles.getSigmaHeading());
*/
}
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