summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:04 +0100
committerGuillaume Seguin <guillaume@segu.in>2008-02-13 12:10:04 +0100
commitf58031717ba0305967c70c6b8527d481375b0df1 (patch)
tree7a786a8cbb3f9e0492b44255e887e181daca1661
parent5ab92c8656d4db62e6fa9b9520a64bf3c4d65c2a (diff)
downloadsegway-f58031717ba0305967c70c6b8527d481375b0df1.tar.gz
segway-f58031717ba0305967c70c6b8527d481375b0df1.tar.bz2
* Backup
-rw-r--r--Segway.java71
1 files changed, 18 insertions, 53 deletions
diff --git a/Segway.java b/Segway.java
index 3d06292..652c287 100644
--- a/Segway.java
+++ b/Segway.java
@@ -26,47 +26,13 @@ import lejos.nxt.*;
import java.util.*;
import java.lang.*;
-class Store extends Thread
-{
- 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;
+ float T_SCALE = 500.0f;
+ float OFFSET = 618.041f;
+ //Datalogger logger;
Store store;
GyroSensor gyro;
@@ -79,10 +45,8 @@ class SegwayRunner extends Thread
public SegwayRunner (SensorPort port)
{
gyro = new GyroSensor (port);
- logger = new Datalogger ();
- store = new Store (gyro);
- Motor.A.smoothAcceleration (false);
- Motor.C.smoothAcceleration (false);
+ //Motor.A.smoothAcceleration (false);
+ //Motor.C.smoothAcceleration (false);
}
/**
@@ -96,36 +60,30 @@ class SegwayRunner extends Thread
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++)
+ for (int i = 0; i < 5000; i++)
{
total += gyro.getRawRotation ();
count += 1;
try { Thread.sleep (DELAY); } catch (Exception e) {}
}
- gyro.setOffset (total / (float) count);*/
-
- //store.start ();
+ gyro.setOffset (total / (float) count);
/* This is the main loop */
while (!Button.ESCAPE.isPressed ())
{
try { Thread.sleep (DELAY); } catch (Exception e) {}
- /* Compute current angle (integration) */
- dt = ((float) System.currentTimeMillis () - t_old) / T_SCALE;
r2 = r1;
r1 = gyro.getRotation ();
- logger.writeLog (r1);
- angle = angle + dt * (r1 + r2) / 2f;
- t_old = System.currentTimeMillis ();
+ /* Compute current angle (integration) */
+ //dt = ((float) System.currentTimeMillis () - t_old) / 1000.0f;
+ angle = angle + (r1 + r2) / 200.0f;
//angle_bias = angle_bias * 0.999f + angle * 0.001f;
@@ -134,8 +92,10 @@ class SegwayRunner extends Thread
/* Set motors direction & speed */
//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
@@ -143,6 +103,7 @@ class SegwayRunner extends Thread
*/
//speed = (int) (300.0f * store.getAverage ());
//speed = 80 * (int) (angle - angle_bias);
+ /*
Motor.A.setSpeed (speed);
Motor.C.setSpeed (speed);
if (angle > 0)
@@ -160,11 +121,13 @@ class SegwayRunner extends Thread
Motor.A.stop ();
Motor.C.stop ();
}
+ */
}
/* Correctly stop the motors before dying */
+ /*
Motor.A.stop ();
Motor.C.stop ();
- //logger.transmit (true);
+ */
}
/**
@@ -188,6 +151,8 @@ class SegwayRunner extends Thread
LCD.drawInt ((int) gyro.getRawRotation (), 0, 1);
LCD.drawString ("Angle:", 0, 2);
LCD.drawInt ((int) angle, 0, 3);
+ //LCD.drawString ("Offset:", 0, 4);
+ LCD.drawInt ((int) (gyro.offset * 10.0f), 0, 4);
//LCD.drawInt ((int) store.getAverage (), 0, 1);
LCD.refresh ();
}