* Commit current state, jan 8 2008, before work

1 files changed, 91 insertions, 22 deletions

diff --git a/Segway.java b/Segway.java
index 57e4be6..a31aa51 100644
--- a/Segway.java
+++ b/Segway.java
@@ -23,14 +23,52 @@
  */
 
 import lejos.nxt.*;
+import java.util.*;
+import java.lang.*;
 
-class SegwayRunner
+class Store extends Thread
 {
-    int         DELAY = 2;
-    float       SCALE = 4.0f;
-    float       T_SCALE = 500.0f;
-    float       OFFSET = 617f;
+    int         DELAY = 20;
 
+    int         COUNT = 5;
+    float       [] values;
+    int         current = 0;
+
+    GyroSensor   gyro;
+
+    public Store (GyroSensor gyro)
+    {
+        this.gyro = gyro;
+        values = new float[5];
+        values[0] = values[1] = values[2] = values[3] = values[4] = 0.0f;
+    }
+
+    public void run ()
+    {
+        while (!Button.ESCAPE.isPressed ())
+        {
+            try { Thread.sleep (DELAY); } catch (Exception e) {}
+            values[current] = gyro.getRotation ();
+            current = (current + 1) % COUNT;
+        }
+    }
+
+    public float getAverage ()
+    {
+        float sum = values[0] + values[1] + values[2] + values[3] + values[4];
+        return sum / 5.0f;
+    }
+}
+
+class SegwayRunner extends Thread
+{
+    int         DELAY = 1;
+    float       SCALE = 1f;
+    float       T_SCALE = 1000.0f;
+    float       OFFSET = 618f;
+    Datalogger  logger;
+
+    Store       store;
     GyroSensor  gyro;
     float       angle = 0.0f;
 
@@ -41,6 +79,8 @@ class SegwayRunner
     public SegwayRunner (SensorPort port)
     {
         gyro = new GyroSensor (port);
+        logger = new Datalogger ();
+        store = new Store (gyro);
         Motor.A.smoothAcceleration (false);
         Motor.C.smoothAcceleration (false);
     }
@@ -48,48 +88,72 @@ class SegwayRunner
     /**
      * Runner main loop: do all the required stuff to keep the robot going
      */
-    public void run () throws Exception
+    public void run ()
     {
         /* 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 angle_bias = 0.0f;
         float r1 = 0, r2 = 0;
         float t_old = System.currentTimeMillis ();
         float dt;
         int speed;
 
+        /*
+        float total = 0.0f;
+        int count = 0;
+        for (int i = 0; i < 500; i++)
+        {
+            total += gyro.getRawRotation ();
+            count += 1;
+            try { Thread.sleep (DELAY); } catch (Exception e) {}
+        }
+        gyro.setOffset (total / (float) count);*/
+
+        //store.start ();
+
         /* This is the main loop */
         while (!Button.ESCAPE.isPressed ())
         {
-            Thread.sleep (DELAY);
-            r2 = gyro.getRotation () / SCALE;
-            Thread.sleep (DELAY);
+            try { Thread.sleep (DELAY); } catch (Exception e) {}
 
-            /* Compute current angle (Runge Kutta integration) */
+            /* Compute current angle (integration) */
             dt = ((float) System.currentTimeMillis () - t_old) / T_SCALE;
-            angle = angle_old + (r1 + 2 * r2) * dt;
-            angle_old = angle;
+            r2 = r1;
+            r1 = gyro.getRotation ();
+            logger.writeLog (r1);
+            angle = angle + dt * (r1 + r2) / 2f;
 
-            r1 = gyro.getRotation () / SCALE;
             t_old = System.currentTimeMillis ();
 
+            //angle_bias = angle_bias * 0.999f + angle * 0.001f;
+
             /* Useful for debugging */
             printAngle ();
 
             /* Set motors direction & speed */
-            speed = Math.abs ((int) angle) * 20;
+            //speed = (int) Math.sqrt ((double) Math.abs ((int) angle)) * 100;
+            speed = Math.abs ((int) angle);
+            speed = 300 * speed;
+            /*if (speed < 10)
+                speed = 300 * speed;
+            else
+                speed = 100 * (int) Math.pow ((double) speed, 3.0);
+            */
+            //speed = (int) (300.0f * store.getAverage ());
+            //speed = 80 * (int) (angle - angle_bias);
             Motor.A.setSpeed (speed);
             Motor.C.setSpeed (speed);
             if (angle > 0)
             {
-                Motor.A.backward ();
-                Motor.C.backward ();
+                Motor.A.forward ();
+                Motor.C.forward ();
             }
             else if (angle < 0)
             {
-                Motor.A.forward ();
-                Motor.C.forward ();
+                Motor.A.backward ();
+                Motor.C.backward ();
             }
             else
             {
@@ -100,6 +164,7 @@ class SegwayRunner
         /* Correctly stop the motors before dying */
         Motor.A.stop ();
         Motor.C.stop ();
+        logger.transmit (true);
     }
 
     /**
@@ -119,8 +184,11 @@ class SegwayRunner
     void printAngle ()
     {
         LCD.clear ();
-        LCD.drawString ("Angle:", 0, 0);
-        LCD.drawInt ((int) angle, 0, 1);
+        LCD.drawString ("Rotation:", 0, 0);
+        LCD.drawInt ((int) gyro.getRawRotation (), 0, 1);
+        LCD.drawString ("Angle:", 0, 2);
+        LCD.drawInt ((int) angle, 0, 3);
+        //LCD.drawInt ((int) store.getAverage (), 0, 1);
         LCD.refresh ();
     }
 }
@@ -136,8 +204,9 @@ public class Segway
      */
     public static void main (String[] args) throws Exception
     {
-        SegwayRunner runner = new SegwayRunner (SensorPort.S1);
-        runner.run ();
+        SegwayRunner runner = new SegwayRunner (SensorPort.S3);
+        runner.start ();
+		Button.ENTER.waitForPressAndRelease ();
     }
 
 }