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
2dad0faf
Commit
2dad0faf
authored
Mar 26, 2015
by
Casper
Browse files
added color sensor segway
parent
3d6f024f
Changes
2
Hide whitespace changes
Inline
Side-by-side
lesson5/DataLogger.java
0 → 100644
View file @
2dad0faf
import
lejos.nxt.*
;
import
java.io.*
;
/**
* A simple data logger that can be used to sample a sequence
* of integer data values in a flash file. The
* file is a text consisting of a sequence of lines with
* a time value and a sample data value separated by a comma.
*
* When data has been sampled the flash file can be transfered to a
* PC by means of the tool nxjbrowse.
*
* @author Ole Caprani
* @version 4.02.13
*/
public
class
DataLogger
{
private
File
f
;
private
FileOutputStream
fos
;
private
int
startTime
;
public
DataLogger
(
String
fileName
)
{
try
{
f
=
new
File
(
fileName
);
if
(
!
f
.
exists
()
)
{
f
.
createNewFile
();
}
else
{
f
.
delete
();
f
.
createNewFile
();
}
fos
=
new
FileOutputStream
(
f
);
}
catch
(
IOException
e
)
{
LCD
.
drawString
(
e
.
getMessage
(),
0
,
0
);
System
.
exit
(
0
);
}
startTime
=
(
int
)
System
.
currentTimeMillis
();
}
public
void
start
()
{
startTime
=
(
int
)
System
.
currentTimeMillis
();
}
public
void
writeSample
(
int
time
,
int
sample
)
{
try
{
Integer
timeInt
=
new
Integer
(
time
);
String
timeString
=
timeInt
.
toString
();
Integer
sampleInt
=
new
Integer
(
sample
);
String
sampleString
=
sampleInt
.
toString
();
for
(
int
i
=
0
;
i
<
timeString
.
length
();
i
++)
{
fos
.
write
((
byte
)
timeString
.
charAt
(
i
));
}
// Separate items with comma
fos
.
write
((
byte
)(
','
));
for
(
int
i
=
0
;
i
<
sampleString
.
length
();
i
++)
{
fos
.
write
((
byte
)
sampleString
.
charAt
(
i
));
}
// New line
fos
.
write
((
byte
)(
'\r'
));
fos
.
write
((
byte
)(
'\n'
));
}
catch
(
IOException
e
)
{
LCD
.
drawString
(
e
.
getMessage
(),
0
,
0
);
System
.
exit
(
0
);
}
}
public
void
writeSample
(
int
sample
)
{
writeSample
((
int
)
System
.
currentTimeMillis
()
-
startTime
,
sample
);
}
public
void
close
()
{
try
{
fos
.
close
();
}
catch
(
IOException
e
)
{
LCD
.
drawString
(
e
.
getMessage
(),
0
,
0
);
System
.
exit
(
0
);
}
}
}
lesson5/SeqwayColor.java
0 → 100644
View file @
2dad0faf
import
lejos.nxt.*
;
public
class
SeqwayColor
{
// Global vars:
int
offset
;
int
prev_error
;
float
int_error
;
ColorSensor
cs
;
static
DataLogger
dl
;
// PID constants
final
int
KP
=
45
;
final
int
KI
=
0
;
final
int
KD
=
1
;
final
int
SCALE
=
1
;
public
SeqwayColor
()
{
cs
=
new
ColorSensor
(
SensorPort
.
S3
);
}
public
void
getBalancePos
()
{
// Wait for user to balance and press orange button
while
(!
Button
.
ENTER
.
isDown
())
{
// NXTway must be balanced.
offset
=
cs
.
getNormalizedLightValue
();
LCD
.
clear
();
LCD
.
drawInt
(
offset
,
2
,
4
);
LCD
.
refresh
();
}
}
public
void
pidControl
()
{
while
(!
Button
.
ESCAPE
.
isDown
())
{
int
normVal
=
cs
.
getNormalizedLightValue
();
// Proportional Error:
int
error
=
normVal
-
offset
;
// Integral Error:
int_error
=
int_error
+
error
;
// Derivative Error:
int
deriv_error
=
error
-
prev_error
;
prev_error
=
error
;
int
pid_val
=
(
int
)(
KP
*
error
+
KI
*
int_error
+
KD
*
deriv_error
)
/
SCALE
;
if
(
pid_val
>
100
)
pid_val
=
100
;
if
(
pid_val
<
-
100
)
pid_val
=
-
100
;
// Power derived from PID value:
int
power
=
Math
.
abs
(
pid_val
);
power
=
55
+
(
power
*
45
)
/
100
;
// NORMALIZE POWER
// dl.writeSample(error);
// dl.writeSample((int)int_error);
// dl.writeSample(deriv_error);
// dl.writeSample(pid_val);
if
(
pid_val
>
0
)
{
MotorPort
.
B
.
controlMotor
(
power
,
BasicMotorPort
.
FORWARD
);
MotorPort
.
C
.
controlMotor
(
power
,
BasicMotorPort
.
FORWARD
);
}
else
{
MotorPort
.
B
.
controlMotor
(
power
,
BasicMotorPort
.
BACKWARD
);
MotorPort
.
C
.
controlMotor
(
power
,
BasicMotorPort
.
BACKWARD
);
}
}
}
public
void
shutDown
()
{
// Shut down light sensor, motors
Motor
.
B
.
flt
();
Motor
.
C
.
flt
();
cs
.
setFloodlight
(
false
);
// dl.close();
}
public
static
void
main
(
String
args
[])
{
// dl = new DataLogger("data.txt");
// dl.start();
SeqwayColor
sw
=
new
SeqwayColor
();
sw
.
getBalancePos
();
sw
.
pidControl
();
sw
.
shutDown
();
}
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment