summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2007-12-01 19:05:56 +0100
committerGuillaume Seguin <guillaume@segu.in>2007-12-01 19:05:56 +0100
commitf0aea6a044daf3380e1dd4b553cf9431d5df7ae7 (patch)
treeab28e0826fb81794a9840faefc80ffc6ee7f8b9a
parent5de94f4d4723cdfed2f502988d81002b85367970 (diff)
downloadsegway-f0aea6a044daf3380e1dd4b553cf9431d5df7ae7.tar.gz
segway-f0aea6a044daf3380e1dd4b553cf9431d5df7ae7.tar.bz2
* Comments
-rw-r--r--Segway.java26
1 files changed, 26 insertions, 0 deletions
diff --git a/Segway.java b/Segway.java
index 4679a0d..57e4be6 100644
--- a/Segway.java
+++ b/Segway.java
@@ -34,6 +34,10 @@ class SegwayRunner
GyroSensor gyro;
float angle = 0.0f;
+ /**
+ * Create a Runner object attached to the specified port.
+ * @param port port, e.g. Port.S1
+ */
public SegwayRunner (SensorPort port)
{
gyro = new GyroSensor (port);
@@ -41,8 +45,13 @@ class SegwayRunner
Motor.C.smoothAcceleration (false);
}
+ /**
+ * Runner main loop: do all the required stuff to keep the robot going
+ */
public void run () throws Exception
{
+ /* Set the Gyro Sensor offset
+ * FIXME: we could have a pre-loop computing the offset live */
gyro.setOffset (OFFSET);
float angle_old = 0.0f;
float r1 = 0, r2 = 0;
@@ -50,12 +59,14 @@ class SegwayRunner
float dt;
int speed;
+ /* This is the main loop */
while (!Button.ESCAPE.isPressed ())
{
Thread.sleep (DELAY);
r2 = gyro.getRotation () / SCALE;
Thread.sleep (DELAY);
+ /* Compute current angle (Runge Kutta integration) */
dt = ((float) System.currentTimeMillis () - t_old) / T_SCALE;
angle = angle_old + (r1 + 2 * r2) * dt;
angle_old = angle;
@@ -63,8 +74,10 @@ class SegwayRunner
r1 = gyro.getRotation () / SCALE;
t_old = System.currentTimeMillis ();
+ /* Useful for debugging */
printAngle ();
+ /* Set motors direction & speed */
speed = Math.abs ((int) angle) * 20;
Motor.A.setSpeed (speed);
Motor.C.setSpeed (speed);
@@ -84,10 +97,14 @@ class SegwayRunner
Motor.C.stop ();
}
}
+ /* Correctly stop the motors before dying */
Motor.A.stop ();
Motor.C.stop ();
}
+ /**
+ * Display current rotation speed (obtained from Gyro)
+ */
void printRotation ()
{
LCD.clear ();
@@ -96,6 +113,9 @@ class SegwayRunner
LCD.refresh ();
}
+ /**
+ * Display currently computed angle
+ */
void printAngle ()
{
LCD.clear ();
@@ -105,9 +125,15 @@ class SegwayRunner
}
}
+/**
+ * main () class
+ */
public class Segway
{
+ /**
+ * Run app: create a Runner object and run it
+ */
public static void main (String[] args) throws Exception
{
SegwayRunner runner = new SegwayRunner (SensorPort.S1);