diff options
author | Guillaume Seguin <guillaume@segu.in> | 2008-01-08 14:11:45 +0100 |
---|---|---|
committer | Guillaume Seguin <guillaume@segu.in> | 2008-01-08 14:11:45 +0100 |
commit | f5d2e87aa177cba3964cad8f226c74e33e16ff98 (patch) | |
tree | fc347706732dc9bb8a4bd4c6604974c2f9b8cda0 | |
parent | f0aea6a044daf3380e1dd4b553cf9431d5df7ae7 (diff) | |
download | segway-f5d2e87aa177cba3964cad8f226c74e33e16ff98.tar.gz segway-f5d2e87aa177cba3964cad8f226c74e33e16ff98.tar.bz2 |
* Commit current state, jan 8 2008, before work
-rw-r--r-- | Segway.java | 113 |
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 (); } } |