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
e80a9cd4
Commit
e80a9cd4
authored
Mar 27, 2015
by
Peder Kronsgaard Detlefsen
Browse files
Added GyroTest and SejwayGyro.
parent
3f268dbf
Changes
2
Hide whitespace changes
Inline
Side-by-side
lesson5/GyroTest.java
0 → 100644
View file @
e80a9cd4
import
lejos.nxt.Button
;
import
lejos.nxt.LCD
;
import
lejos.nxt.MotorPort
;
import
lejos.nxt.SensorPort
;
import
lejos.nxt.addon.GyroSensor
;
/**
* Test of Gyro Sensor
* Records the minimum, maximum and current values
*
* @author Lawrie Griffiths
*/
public
class
GyroTest
{
public
static
void
main
(
String
[]
args
)
throws
Exception
{
GyroSensor
gyro
=
new
GyroSensor
(
SensorPort
.
S2
);
float
minValue
=
1023
,
maxValue
=
0
;
int
sampleInterval
=
5
;
// ms
LCD
.
drawString
(
"Get ready for calibrating."
,
0
,
0
);
Button
.
waitForAnyPress
();
Car
.
forward
(
50
,
50
);
Thread
.
sleep
(
5000
);
LCD
.
clear
();
LCD
.
drawString
(
"Calibrating. Don't move!"
,
0
,
0
);
gyro
.
recalibrateOffset
();
LCD
.
clear
();
LCD
.
drawString
(
"Done Calibrating."
,
0
,
0
);
Button
.
waitForAnyPress
();
LCD
.
clear
();
LCD
.
drawString
(
"Gyro Test:"
,
0
,
0
);
LCD
.
drawString
(
"Min:"
,
0
,
2
);
LCD
.
drawString
(
"Max:"
,
0
,
3
);
LCD
.
drawString
(
"Current:"
,
0
,
4
);
while
(!
Button
.
ESCAPE
.
isDown
())
{
float
value
=
gyro
.
readValue
();
minValue
=
Math
.
min
(
minValue
,
value
);
maxValue
=
Math
.
max
(
maxValue
,
value
);
LCD
.
drawInt
((
int
)
minValue
,
6
,
5
,
2
);
LCD
.
drawInt
((
int
)
maxValue
,
6
,
5
,
3
);
LCD
.
drawInt
((
int
)(
value
),
6
,
9
,
4
);
Thread
.
sleep
(
sampleInterval
);
}
}
}
lesson5/SejwayGyro.java
0 → 100644
View file @
e80a9cd4
import
lejos.nxt.*
;
import
lejos.nxt.addon.GyroSensor
;
import
lejos.util.Delay
;
public
class
SejwayGyro
{
private
static
final
float
KGYROANGLE
=
7.5f
;
private
static
final
float
KGYROSPEED
=
15.15f
;
private
static
final
float
KPOS
=
0.07f
;
private
static
final
float
KSPEED
=
0.1f
;
GyroSensor
gyro
;
private
float
power
;
private
float
gyroSpeed
;
private
float
gyroAngle
;
private
int
motorPos
;
private
int
motorSpeed
;
private
float
tInterval
=
0.01f
;
private
float
globalAngle
;
public
SejwayGyro
()
{
gyro
=
new
GyroSensor
(
SensorPort
.
S2
);
}
public
void
getBalancePos
()
{
LCD
.
drawString
(
"Put down NXT"
,
0
,
0
);
Button
.
waitForAnyPress
();
LCD
.
drawString
(
"Calibrating..."
,
0
,
0
);
gyro
.
recalibrateOffset
();
MotorPort
.
B
.
resetTachoCount
();
MotorPort
.
C
.
resetTachoCount
();
LCD
.
drawString
(
"Balance NXT"
,
0
,
0
);
Button
.
waitForAnyPress
();
}
public
void
pidControl
()
{
while
(!
Button
.
ESCAPE
.
isDown
())
{
updateGyroData
(
gyro
);
updateMotorData
();
power
=
KGYROSPEED
*
gyroSpeed
;
//KGYROANGLE * gyroAngle;
//KPOS * motorPos +
//KSPEED * motorSpeed;
MotorPort
.
B
.
controlMotor
((
int
)
power
,
BasicMotorPort
.
FORWARD
);
MotorPort
.
C
.
controlMotor
((
int
)
power
,
BasicMotorPort
.
FORWARD
);
LCD
.
clear
();
LCD
.
drawString
(
"gyroSpeed:"
,
0
,
1
);
LCD
.
drawString
(
"gyroAngle:"
,
0
,
2
);
LCD
.
drawString
(
"motorPos:"
,
0
,
3
);
LCD
.
drawString
(
"motorSpeed:"
,
0
,
4
);
LCD
.
drawString
(
"power:"
,
0
,
5
);
LCD
.
drawInt
((
int
)
gyroSpeed
,
10
,
1
);
LCD
.
drawInt
((
int
)
gyroAngle
,
4
,
10
,
2
);
LCD
.
drawInt
(
motorPos
,
4
,
10
,
3
);
LCD
.
drawInt
(
motorSpeed
,
4
,
10
,
4
);
LCD
.
drawInt
((
int
)
power
,
4
,
10
,
5
);
LCD
.
refresh
();
Delay
.
msDelay
(
10
);
}
}
private
void
updateMotorData
()
{
motorPos
=
Motor
.
B
.
getTachoCount
();
motorSpeed
=
Motor
.
B
.
getRotationSpeed
();
}
private
void
updateGyroData
(
GyroSensor
gyro
)
{
gyroSpeed
=
gyro
.
getAngularVelocity
();
globalAngle
+=
gyroSpeed
*
tInterval
;
gyroAngle
=
globalAngle
;
}
public
static
void
main
(
String
[]
args
)
{
SejwayGyro
sej
=
new
SejwayGyro
();
sej
.
getBalancePos
();
sej
.
pidControl
();
}
}
\ No newline at end of file
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