Mindboards

Code: Select all

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.GyroSensor;


/**
 * This is an experimental test class to study the HiTechnic Gyro Sensor for use as a gyro compass..
 * 
 * The class is not in a finished state, and is only meant for experiments.
 * 
 * @author kvols
 *
 */
public class GyroCompassTest implements Runnable {

	public static final int SAMPLE_RATE= 5;
	
	private final GyroSensor gs;
	// Heading in 1/1000 degrees
	private int heading = 0;
	// Sensor offset in 1/1000 degrees/s
	int sensorOffset;

	public GyroCompassTest(GyroSensor gs) throws InterruptedException {
		this.gs = gs;
		// Find a reasonable offset
		for (int i = 0; i < 20; i++) {
			sensorOffset += gs.readValue();
			Thread.sleep(3);
		}
		sensorOffset *= 50;
		// Start the sensor sampler
		Thread t = new Thread(this);
		t.setDaemon(true);
		t.start();
	}

	/**
	 * @throws InterruptedException
	 */
	public static void main(String[] args) throws InterruptedException {
		GyroSensor gs = new GyroSensor(SensorPort.S1);
		GyroCompassTest gt = new GyroCompassTest(gs);

		while (!Button.ESCAPE.isPressed()) {
				if (Button.ENTER.isPressed()) // Set to zero
					gt.heading = 0;
			Thread.sleep(25);
			LCD.clear();
			// Display the heading rounded to nearest degree:
			LCD.drawInt(gt.sensorOffset, 5, 5);
			// Display the current estimated sensor offset (1000nds):
			LCD.drawInt((gt.heading+500)/1000, 5, 3);
		}
	}

	@Override
	public void run() {
		int av = 0;
		int oldAv;
		int err = 0;
		int errCount = 0;
		long time = System.currentTimeMillis();
		try {
			Thread.sleep(SAMPLE_RATE);
			for (;;) {
				oldAv = av;
				av = gs.readValue() * 1000 - sensorOffset;
				// Detect no movement and fine tune the offset
				if (av >= -1000 && av <= 1000) {
					err += av;
					errCount++;
					// Tune the offset if still for some time
					if (errCount == 30) {
						// Let the offset converge to prevent over-compensation:
						sensorOffset += err / (errCount*2);
						// Adjust the current heading, to prevent idle drifting:
						av = -err;
						errCount = 0;
						err = 0;
					}
				} else {
					errCount = 0;
					err = 0;
				}
				// Calculate the absolute rotation
				int dt = (int) (System.currentTimeMillis() - time);
				time += dt;
				heading += ((oldAv + av) * dt) / 2000;
				// Sleep or some time
				Thread.sleep(SAMPLE_RATE);
			}
		} catch (InterruptedException e) { // Just stop if interrupted!
		}
	}
}

Code: Select all

 time += dt;
heading += ((oldAv + av) * dt) / 2000;