Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
LEGO Group 8
LEGO
Commits
f55f1f8a
Commit
f55f1f8a
authored
Jun 17, 2015
by
Peder Kronsgaard Detlefsen
Browse files
Added final result.
parent
a711a3a4
Changes
14
Hide whitespace changes
Inline
Side-by-side
lesson10/NXT/PilotAvoid.java
View file @
f55f1f8a
import
java.io.File
;
import
java.util.Vector
;
import
lejos.geom.Point
;
import
lejos.nxt.*
;
import
lejos.nxt.comm.RConsole
;
import
lejos.robotics.localization.OdometryPoseProvider
;
import
lejos.robotics.navigation.DifferentialPilot
;
import
lejos.robotics.navigation.Move
;
import
lejos.robotics.navigation.Pose
;
import
lejos.util.Delay
;
public
class
PilotAvoid
{
private
SlaveIOStreams
PC
;
...
...
@@ -14,13 +22,24 @@ public class PilotAvoid {
DifferentialPilot
pilot
=
new
DifferentialPilot
(
leftWheelDiameter
,
rightWheelDiameter
,
trackWidth
,
left
,
right
,
false
);
public
OdometryPoseProvider
poseProvider
=
new
OdometryPoseProvider
(
pilot
);
public
Point
desPoint
;
public
PilotAvoid
(
boolean
usb
)
{
public
PilotAvoid
(
boolean
usb
,
float
desX
,
float
desY
)
{
PC
=
new
SlaveIOStreams
(
usb
);
PC
.
open
();
// Send destination to PC.
PC
.
output
(
desX
);
PC
.
output
(
desY
);
pilot
.
setTravelSpeed
(
travelSpeed
);
pilot
.
setRotateSpeed
(
rotateSpeed
);
Pose
initialPose
=
new
Pose
(
0
,
0
,
0
);
poseProvider
.
setPose
(
initialPose
);
desPoint
=
new
Point
(
desX
,
desY
);
}
public
void
sendMove
(
Move
move
)
{
...
...
@@ -29,74 +48,39 @@ public class PilotAvoid {
PC
.
output
(
move
.
getAngleTurned
());
}
public
void
travel
(
double
distance
)
{
pilot
.
travel
(
distance
,
true
);
}
public
void
rotate
(
double
angle
)
{
pilot
.
rotate
(
angle
,
true
);
public
void
travel
(
double
distance
,
boolean
immediateReturn
)
{
pilot
.
travel
(
distance
,
immediateReturn
);
}
public
void
go
()
{
Sound
.
beep
();
while
(!
Button
.
ENTER
.
isDown
())
Thread
.
yield
();
Sound
.
twoBeeps
();
travel
(
500
);
rotate
(
90
);
travel
(
200
);
rotate
(-
90
);
travel
(
500
);
/*
* travel(500); rotate(90); travel(200); rotate(-90); travel(500);
* rotate(90); travel(200); rotate(90); travel(700); rotate(90);
* travel(200);
*/
while
(!
Button
.
ENTER
.
isDown
())
Thread
.
yield
();
LCD
.
clear
();
LCD
.
drawString
(
"Closing"
,
0
,
0
);
if
(
PC
.
close
())
LCD
.
drawString
(
"Closed"
,
0
,
0
);
try
{
Thread
.
sleep
(
2000
);
}
catch
(
Exception
e
)
{
}
}
public
void
rotate
(
double
angle
,
boolean
immediateReturn
)
{
pilot
.
rotate
(
angle
,
immediateReturn
);
}
public
static
void
main
(
String
[]
args
)
{
PilotAvoid
pilot
=
new
PilotAvoid
(
false
);
PilotAvoid
pilot
=
new
PilotAvoid
(
false
,
1000
,
500
);
LCD
.
clear
();
LCD
.
drawString
(
"PilotRouteMM"
,
0
,
0
);
// route.go();
LCD
.
drawString
(
"PilotAvoid"
,
0
,
0
);
Sound
.
beep
();
while
(!
Button
.
ENTER
.
isDown
())
Thread
.
yield
();
Sound
.
twoBeeps
();
Behavior
b1
=
new
FollowRoute
(
pilot
);
Behavior
b2
=
new
AvoidObstacle
(
pilot
);
Behavior
[]
behaviorList
=
{
b1
,
b2
};
Behavior
b1
=
new
FindGoal
(
pilot
);
Behavior
b2
=
new
AvoidObstacle
(
pilot
);
Behavior
b3
=
new
RecognizeGoal
(
pilot
);
Behavior
[]
behaviorList
=
{
b1
,
b2
,
b3
};
Arbitrator
arbitrator
=
new
Arbitrator
(
behaviorList
);
arbitrator
.
start
();
}
}
class
FollowRoute
implements
Behavior
{
private
static
final
int
TRAVEL
=
0
;
private
static
final
int
ROTATE
=
1
;
class
FindGoal
implements
Behavior
{
private
boolean
_suppressed
=
false
;
private
PilotAvoid
pilot
;
private
int
stepsDone
=
0
;
public
F
ollowRoute
(
PilotAvoid
pilot
)
{
public
F
indGoal
(
PilotAvoid
pilot
)
{
this
.
pilot
=
pilot
;
}
...
...
@@ -110,62 +94,66 @@ class FollowRoute implements Behavior {
public
void
action
()
{
_suppressed
=
false
;
Sound
.
beepSequence
();
Pose
p
=
pilot
.
poseProvider
.
getPose
();
if
(
stepsDone
<
1
&&
!
_suppressed
)
{
doStep
(
TRAVEL
,
500
);
}
if
(
stepsDone
<
2
&&
!
_suppressed
)
{
doStep
(
ROTATE
,
90
);
}
if
(
stepsDone
<
3
&&
!
_suppressed
)
{
doStep
(
TRAVEL
,
200
);
}
if
(
stepsDone
<
4
&&
!
_suppressed
)
{
doStep
(
ROTATE
,
-
90
);
}
if
(
stepsDone
<
5
&&
!
_suppressed
)
{
doStep
(
TRAVEL
,
500
);
}
// Calculate angle based on destination.
float
angle
=
p
.
angleTo
(
pilot
.
desPoint
);
// float degrees = (angle > 0) ? angle - p.getHeading() : angle + p.getHeading();
float
degrees
=
-
p
.
getHeading
()
+
angle
;
Motor
.
A
.
stop
(
true
);
// not strictly necessary, but good programming practice
Motor
.
C
.
stop
(
true
);
}
public
void
doStep
(
int
moveType
,
double
value
)
{
switch
(
moveType
)
{
case
TRAVEL
:
pilot
.
travel
(
value
);
break
;
case
ROTATE
:
pilot
.
rotate
(
value
);
break
;
default
:
System
.
err
.
println
(
"Invalid moveType in doStep(): "
+
moveType
);
return
;
}
stepsDone
++;
// if (angle < 0 && p.getHeading() < 0) {
// degrees = angle - p.getHeading();
// } else if (angle > 0 && p.getHeading() < 0) {
// degrees = angle + p.getHeading();
// } else if (angle > 0 && p.getHeading() > 0) {
// degrees = angle + p.getHeading();
// } else if (angle < 0 && p.getHeading() > 0) {
// degrees = angle - p.getHeading(); // done
// }
pilot
.
rotate
(
degrees
,
false
);
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
pilot
.
travel
(
p
.
distanceTo
(
pilot
.
desPoint
),
true
);
while
(!
_suppressed
&&
Motor
.
A
.
isMoving
())
{
Thread
.
yield
();
// don't exit till suppressed
Thread
.
yield
();
}
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
}
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
Motor
.
A
.
stop
(
true
);
// not strictly necessary, but good programming practice
Motor
.
C
.
stop
(
true
);
}
}
class
AvoidObstacle
extends
Thread
implements
Behavior
{
UltrasonicSensor
sonar
=
new
UltrasonicSensor
(
SensorPort
.
S1
);
private
float
distance
=
255
;
UltrasonicSensor
sonar
=
new
UltrasonicSensor
(
SensorPort
.
S2
);
private
int
distance
=
255
;
private
int
count
=
0
;
private
boolean
active
=
false
;
private
boolean
_suppressed
=
false
;
private
PilotAvoid
pilot
;
public
AvoidObstacle
(
PilotAvoid
pilot
)
{
this
.
pilot
=
pilot
;
this
.
i
sDaemon
();
this
.
s
et
Daemon
(
true
);
this
.
start
();
}
public
void
run
()
{
while
(
true
)
{
distance
=
sonar
.
getDistance
();
LCD
.
drawInt
(
distance
,
3
,
0
,
2
);
}
}
public
int
takeControl
()
{
return
(
distance
<
15
)
?
20
:
0
;
count
++;
int
t
=
(
distance
<
25
||
active
)
?
20
:
0
;
LCD
.
drawInt
(
t
,
2
,
0
,
3
);
LCD
.
drawInt
(
count
,
6
,
0
,
4
);
return
t
;
}
public
void
suppress
()
{
...
...
@@ -174,15 +162,75 @@ class AvoidObstacle extends Thread implements Behavior {
public
void
action
()
{
_suppressed
=
false
;
active
=
true
;
Sound
.
buzz
();
// Look for a clear passage.
pilot
.
rotate
(
45
,
true
);
while
(!
_suppressed
&&
Motor
.
A
.
isMoving
())
{
Thread
.
yield
();
}
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
float
disLeft
=
sonar
.
getDistance
()
*
10
;
pilot
.
rotate
(-
90
,
true
);
while
(!
_suppressed
&&
Motor
.
A
.
isMoving
())
{
Thread
.
yield
();
}
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
float
disRight
=
sonar
.
getDistance
()
*
10
;
// Take evasive action.
if
(
disLeft
>
disRight
)
{
pilot
.
rotate
(
90
,
true
);
while
(!
_suppressed
&&
Motor
.
A
.
isMoving
())
{
Thread
.
yield
();
}
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
}
pilot
.
travel
(
Math
.
min
(
Math
.
max
(
disLeft
-
200
,
disRight
-
200
),
300
),
true
);
while
(!
_suppressed
)
{
Thread
.
yield
();
// don't exit till suppressed
while
(!
_suppressed
&&
Motor
.
A
.
isMoving
())
{
Thread
.
yield
();
}
// Motor.A.stop(true); // not strictly necessary, but good programming practice
// Motor.C.stop(true);
pilot
.
sendMove
(
pilot
.
pilot
.
getMovement
());
Motor
.
A
.
stop
(
true
);
// not strictly necessary, but good programming practice
Motor
.
C
.
stop
(
true
);
active
=
false
;
}
}
class
RecognizeGoal
implements
Behavior
{
private
boolean
_suppressed
=
false
;
private
PilotAvoid
pilot
;
private
Pose
p
;
public
RecognizeGoal
(
PilotAvoid
pilot
)
{
this
.
pilot
=
pilot
;
}
public
int
takeControl
()
{
// Check if we reached the goal.
p
=
pilot
.
poseProvider
.
getPose
();
float
d
=
p
.
distanceTo
(
pilot
.
desPoint
);
LCD
.
drawInt
((
int
)
d
,
5
,
0
,
5
);
return
(
d
<=
10
)
?
50
:
0
;
}
public
void
suppress
()
{
_suppressed
=
true
;
// standard practice for suppress methods
}
public
void
action
()
{
_suppressed
=
false
;
// We reached the target!
Sound
.
beepSequenceUp
();
while
(!
Button
.
ESCAPE
.
isDown
())
{
Delay
.
msDelay
(
100
);
}
System
.
exit
(
0
);
}
}
\ No newline at end of file
lesson10/PC/InputOutputStreams.java
View file @
f55f1f8a
...
...
@@ -29,7 +29,7 @@ public class InputOutputStreams
}
else
{
// Connect to any NXT using Bluetooth
if
(!
conn
.
connectTo
(
"btspp://"
)){
if
(!
conn
.
connectTo
(
"btspp://
NXT8
"
)){
return
"No NXT found using Bluetooth"
;
}
}
...
...
lesson10/PC/PilotGUI.java
View file @
f55f1f8a
...
...
@@ -21,8 +21,8 @@ public class PilotGUI extends JFrame {
// program will automatically adjust if these three constants are changed
static
final
int
VIS_HEIGHT
=
1080
;
// visualization height
static
final
int
VIS_WIDTH
=
1920
;
// visualization width
static
final
int
Y_OFFSET
=
3
0
;
// vertical offset to visualization
static
final
int
X_OFFSET
=
20
;
// horizontal offset to visualization
static
final
int
Y_OFFSET
=
20
0
;
// vertical offset to visualization
static
final
int
X_OFFSET
=
20
0
;
// horizontal offset to visualization
static
final
int
POINT_WIDTH
=
1
;
// width of points shown in the visualization
// VIS_HEIGHT + 2 * Y_OFFSET is the window height
// VIS_WIDTH + 2 * X_OFFSET is the window width
...
...
@@ -34,6 +34,8 @@ public class PilotGUI extends JFrame {
ArrayList
<
Point
>
route
;
// the route as seen from the vehicle
ArrayList
<
ParticleSet
>
particleSets
=
new
ArrayList
<
ParticleSet
>();
private
int
desX
;
private
int
desY
;
// an inner class to handle the visualization window being closed manually
class
MyWindowAdapter
extends
WindowAdapter
{
...
...
@@ -43,9 +45,11 @@ public class PilotGUI extends JFrame {
}
// the constructor for the Visualize class
public
PilotGUI
(
ParticleSet
particles
)
public
PilotGUI
(
ParticleSet
particles
,
int
desX
,
int
desY
)
{
this
.
particles
=
particles
;
this
.
desX
=
desX
;
this
.
desY
=
desY
;
this
.
setSize
(
new
Dimension
(
VIS_WIDTH
+
X_OFFSET
*
2
,
VIS_HEIGHT
+
Y_OFFSET
*
2
));
this
.
setTitle
(
"Monte Carlo Localization"
);
this
.
setVisible
(
true
);
...
...
@@ -101,7 +105,9 @@ public class PilotGUI extends JFrame {
}
}
// Draw destination.
g2
.
setColor
(
Color
.
RED
);
g2
.
fillOval
(
winX
(
desX
)
-
5
,
winY
(
desY
)
+
5
,
10
,
10
);
}
private
int
winX
(
int
x
)
...
...
lesson10/PC/PilotMonitor.java
View file @
f55f1f8a
...
...
@@ -19,18 +19,23 @@ public class PilotMonitor
private
boolean
USB
=
false
;
private
Route
route
=
new
Route
();
private
ParticleSet
particles
=
new
ParticleSet
(
1000
);
private
PilotGUI
view
=
new
PilotGUI
(
particles
)
;
private
PilotGUI
view
;
private
Move
move
;
public
PilotMonitor
()
{
String
m
;
view
.
update
(
route
.
getRoute
(),
false
);
String
m
;
NXT
=
new
InputOutputStreams
(
USB
);
do
{
m
=
NXT
.
open
();
System
.
out
.
println
(
m
);
}
while
(
m
!=
"Connected"
);
// Receive destination from NXT.
int
desX
=
(
int
)
NXT
.
input
();
int
desY
=
(
int
)
NXT
.
input
();
view
=
new
PilotGUI
(
particles
,
desX
,
desY
);
view
.
update
(
route
.
getRoute
(),
false
);
}
private
Move
getMove
()
...
...
lesson11/NXT/ArenaRobot.java
View file @
f55f1f8a
...
...
@@ -54,9 +54,9 @@ public class ArenaRobot {
Behavior
b1
=
new
Wander
(
pilot
);
Behavior
b2
=
new
AvoidEdge
(
pilot
);
Behavior
b3
=
new
Exit
(
pilot
);
Behavior
b3
=
new
Exit
(
pilot
,
180
);
Behavior
[]
behaviorList
=
{
b1
,
b2
,
b3
};
Arbitrator
arbitrator
=
new
Arbitrator
(
behaviorList
);
Arbitrator
arbitrator
=
new
Arbitrator
(
behaviorList
);
arbitrator
.
start
();
}
}
...
...
@@ -85,7 +85,7 @@ class Wander implements Behavior {
double
rand
=
Math
.
random
();
if
(
rand
>
0.5
)
{
pilot
.
travel
(
rand
*
200
0
);
pilot
.
travel
(
rand
*
200
);
}
else
{
pilot
.
rotate
((
Math
.
random
()
>
0.5
)
?
rand
*
180
:
rand
*
-
180
);
}
...
...
@@ -160,13 +160,18 @@ class AvoidEdge extends Thread implements Behavior {
class
Exit
implements
Behavior
{
private
boolean
_suppressed
=
false
;
private
ArenaRobot
pilot
;
private
int
seconds
;
private
long
startTime
;
public
Exit
(
ArenaRobot
pilot
)
{
public
Exit
(
ArenaRobot
pilot
,
int
seconds
)
{
this
.
pilot
=
pilot
;
this
.
seconds
=
seconds
;
startTime
=
System
.
currentTimeMillis
();
}
public
int
takeControl
()
{
return
(
Button
.
ESCAPE
.
isDown
())
?
100
:
0
;
// this behavior always wants control.
return
(
Button
.
ESCAPE
.
isDown
()
||
System
.
currentTimeMillis
()
>
(
startTime
+
seconds
*
1000
))
?
100
:
0
;
}
public
void
suppress
()
{
...
...
lesson11/NXT/LightTest.java
0 → 100644
View file @
f55f1f8a
import
lejos.nxt.Button
;
import
lejos.nxt.LCD
;
import
lejos.nxt.LightSensor
;
import
lejos.nxt.SensorPort
;
public
class
LightTest
{
public
static
void
main
(
String
[]
args
)
{
LightSensor
sensor
=
new
LightSensor
(
SensorPort
.
S1
);
while
(!
Button
.
ESCAPE
.
isDown
())
{
LCD
.
drawString
(
"Light: "
,
0
,
5
);
LCD
.
drawInt
(
sensor
.
readNormalizedValue
(),
4
,
10
,
5
);
}
}
}
lesson11/NXT/PilotControl2D.java
0 → 100644
View file @
f55f1f8a
import
lejos.nxt.*
;
import
lejos.robotics.navigation.DifferentialPilot
;
import
lejos.robotics.navigation.Move
;
import
lejos.util.Delay
;
public
class
PilotControl2D
{
private
SlaveIOStreams
PC
;
private
LightSensor
light
=
new
LightSensor
(
SensorPort
.
S1
);
double
mult
=
1.007
;
double
leftWheelDiameter
=
55
*
mult
,
rightWheelDiameter
=
54.84
*
mult
,
trackWidth
=
165
;
double
travelSpeed
=
50
,
rotateSpeed
=
45
;
NXTRegulatedMotor
left
=
Motor
.
A
;
NXTRegulatedMotor
right
=
Motor
.
C
;
DifferentialPilot
pilot
=
new
DifferentialPilot
(
leftWheelDiameter
,
rightWheelDiameter
,
trackWidth
,
left
,
right
,
false
);
private
boolean
haveLocation
=
false
;
public
PilotControl2D
(
boolean
usb
)
{
PC
=
new
SlaveIOStreams
(
usb
);
PC
.
open
();
pilot
.
setTravelSpeed
(
travelSpeed
);
pilot
.
setRotateSpeed
(
rotateSpeed
);
}
private
void
sendMove
(
Move
move
)
{
PC
.
output
((
move
.
getMoveType
()
==
Move
.
MoveType
.
TRAVEL
?
0
:
1
));
PC
.
output
(
move
.
getDistanceTraveled
());
PC
.
output
(
move
.
getAngleTurned
());
}
private
void
sendLight
()
{
PC
.
output
(
light
.
getLightValue
());
}
private
void
travel
(
double
distance
)
{
pilot
.
travel
(
distance
);
sendMove
(
pilot
.
getMovement
());
PC
.
output
(
light
.
readNormalizedValue
());
}
private
void
rotate
(
double
angle
)
{
pilot
.
rotate
(
angle
);
sendMove
(
pilot
.
getMovement
());
PC
.
output
(
light
.
readNormalizedValue
());
}
public
void
go
()
{
Sound
.
beep
();
while
(
!
Button
.
ENTER
.
isDown
())
Thread
.
yield
();
Sound
.
twoBeeps
();
travel
(
0
);
PC
.
input
();
for
(
int
i
=
0
;
i
<
6
;
i
++)
{
travel
(
115
);
Delay
.
msDelay
(
500
);
if
(
PC
.
input
()
==
1
)
{
// I have my location!
Sound
.
buzz
();
haveLocation
=
true
;
break
;
}
}
if
(!
haveLocation
)
{
rotate
(
90
);
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
travel
(
115
);
Delay
.
msDelay
(
500
);
if
(
PC
.
input
()
==
1
)
{
// I have my location!
Sound
.
buzz
();
haveLocation
=
true
;
break
;
}
}
}
if
(!
haveLocation
)
{
rotate
(
90
);
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
travel
(
115
);
Delay
.
msDelay
(
500
);
if
(
PC
.
input
()
==
1
)
{
// I have my location!
Sound
.
buzz
();
haveLocation
=
true
;
break
;
}
}
}
while
(
!
Button
.
ENTER
.
isDown
())
Thread
.
yield
();
LCD
.
clear
();
LCD
.
drawString
(
"Closing"
,
0
,
0
);
if
(
PC
.
close
()
)
LCD
.
drawString
(
"Closed"
,
0
,
0
);
try
{
Thread
.
sleep
(
2000
);}
catch
(
Exception
e
){}
}
public
static
void
main
(
String
[]
args
)
{
PilotControl2D
route
=
new
PilotControl2D
(
false
);
LCD
.
clear
();
LCD
.
drawString
(
"PilotControl"
,
0
,
0
);
route
.
go
();
}
}
lesson11/NXT/PilotController.java
0 → 100644
View file @
f55f1f8a
import
lejos.nxt.*
;
import
lejos.robotics.navigation.DifferentialPilot
;
import
lejos.robotics.navigation.Move
;
public
class
PilotController
{
private
SlaveIOStreams
PC
;
private
LightSensor
light
=
new
LightSensor
(
SensorPort
.
S1
);
double
mult
=
1.007
;
double
leftWheelDiameter
=
55
*
mult
,
rightWheelDiameter
=
54.84
*
mult
,
trackWidth
=
165
;
double
travelSpeed
=
50
,
rotateSpeed
=
45
;
NXTRegulatedMotor
left
=
Motor
.
A
;
NXTRegulatedMotor
right
=
Motor
.
C
;
DifferentialPilot
pilot
=
new
DifferentialPilot
(
leftWheelDiameter
,
rightWheelDiameter
,
trackWidth
,
left
,
right
,
false
);
public
PilotController
(
boolean
usb
)
{
PC
=
new
SlaveIOStreams
(
usb
);
PC
.
open
();
pilot
.
setTravelSpeed
(
travelSpeed
);
pilot
.
setRotateSpeed
(
rotateSpeed
);
}
private
void
sendMove
(
Move
move
)
{
PC
.
output
((
move
.
getMoveType
()
==
Move
.
MoveType
.
TRAVEL
?
0
:
1
));
PC
.
output
(
move
.
getDistanceTraveled
());
PC
.
output
(
move
.
getAngleTurned
());
}
private
void
travel
(
double
distance
)
{
pilot
.
travel
(
distance
);
sendMove
(
pilot
.
getMovement
());
PC
.
output
(
light
.
readNormalizedValue
());
}
private
void
rotate
(
double
angle
)