Commit ddd9f7a6 authored by lildholdt's avatar lildholdt
Browse files

Monitor and route

parent 03a3171c
......@@ -5,13 +5,14 @@ import lejos.robotics.navigation.Move;
public class PilotRoute
{
private SlaveIOStreams PC;
private double wheelDiameter = 5.5, trackWidth = 16.0;
private double travelSpeed = 5, rotateSpeed = 45;
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 NXTRegulatedMotor left = Motor.B;
private NXTRegulatedMotor right = Motor.C;
private LightSensor lightsensor = new LightSensor(SensorPort.S4);
private DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, left, right);
private DifferentialPilot pilot = new DifferentialPilot(wheelDiameterL,wheelDiameterR,trackWidth, left, right,false);
public PilotRoute(boolean usb)
{
......@@ -27,6 +28,7 @@ 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 travel(double distance)
......@@ -47,11 +49,10 @@ public class PilotRoute
while ( ! Button.ENTER.isDown()) Thread.yield();
Sound.twoBeeps();
travel(50);
rotate(90);
travel(20);
rotate(-90);
travel(50);
for(int i=0;i<40;i++)
{
travel(5);
}
while ( ! Button.ENTER.isDown()) Thread.yield();
......
......@@ -19,7 +19,7 @@ public class RobotMonitor
private Route route = new Route(0,25,0);
private RobotGUI view = new RobotGUI(particles, m);
private Move move;
private int pause = 1000; // ms between views
private int pause = 20; // ms between views
private InputOutputStreams NXT;
private boolean USB = false;
......@@ -71,13 +71,37 @@ public class RobotMonitor
Delay.msDelay(pause);
}
private Move getMove()
{
Move m;
int type = (int)NXT.input();
float distance = NXT.input();
float angle = NXT.input();
m = new Move( (type == 0)? Move.MoveType.TRAVEL : Move.MoveType.ROTATE ,
distance, angle, false);
System.out.println("Move " + distance + " " + angle);
return m;
}
private int getSensorValue()
{
int light = (int)NXT.input();
System.out.println("Light value " + light);
return light;
}
public void goSimulation()
{
for ( int i=0; i < 40; i++)
{
motionUpdate(getMove());
sensorUpdate(getSensorValue());
}
/*
for ( int i=0; i < 16; i++)
{
move = new Move(Move.MoveType.TRAVEL, 9, 0, false);
motionUpdate(move);
motionUpdate(getMove());
sensorUpdate(400);
}
......@@ -103,6 +127,7 @@ public class RobotMonitor
Pose p = particles.getPose();
System.out.println("Position "+p.getX()+" "+p.getY()+" "+p.getHeading());
System.out.println("Accuracy "+ particles.getSigmaX()+ " "+ particles.getSigmaY()+" "+particles.getSigmaHeading());
*/
}
......
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