summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-01-08 14:11:45 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-01-08 14:11:45 +0100
commitf5d2e87aa177cba3964cad8f226c74e33e16ff98 (patch)
treefc347706732dc9bb8a4bd4c6604974c2f9b8cda0
parentf0aea6a044daf3380e1dd4b553cf9431d5df7ae7 (diff)
downloadsegway-f5d2e87aa177cba3964cad8f226c74e33e16ff98.tar.gz
segway-f5d2e87aa177cba3964cad8f226c74e33e16ff98.tar.bz2
* Commit current state, jan 8 2008, before work
-rw-r--r--Segway.java113
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 ();
}
}