Commit e80a9cd4 authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added GyroTest and SejwayGyro.

parent 3f268dbf
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.GyroSensor;
/**
* Test of Gyro Sensor
* Records the minimum, maximum and current values
*
* @author Lawrie Griffiths
*/
public class GyroTest
{
public static void main(String[] args) throws Exception
{
GyroSensor gyro = new GyroSensor(SensorPort.S2);
float minValue = 1023, maxValue = 0;
int sampleInterval = 5; // ms
LCD.drawString("Get ready for calibrating.", 0, 0);
Button.waitForAnyPress();
Car.forward(50, 50);
Thread.sleep(5000);
LCD.clear();
LCD.drawString("Calibrating. Don't move!", 0, 0);
gyro.recalibrateOffset();
LCD.clear();
LCD.drawString("Done Calibrating.", 0, 0);
Button.waitForAnyPress();
LCD.clear();
LCD.drawString("Gyro Test:", 0, 0);
LCD.drawString("Min:", 0, 2);
LCD.drawString("Max:", 0, 3);
LCD.drawString("Current:", 0, 4);
while(!Button.ESCAPE.isDown())
{
float value = gyro.readValue();
minValue = Math.min(minValue, value);
maxValue = Math.max(maxValue, value);
LCD.drawInt((int) minValue, 6, 5, 2);
LCD.drawInt((int) maxValue, 6, 5, 3);
LCD.drawInt((int)(value), 6, 9, 4);
Thread.sleep(sampleInterval);
}
}
}
import lejos.nxt.*;
import lejos.nxt.addon.GyroSensor;
import lejos.util.Delay;
public class SejwayGyro
{
private static final float KGYROANGLE = 7.5f;
private static final float KGYROSPEED = 15.15f;
private static final float KPOS = 0.07f;
private static final float KSPEED = 0.1f;
GyroSensor gyro;
private float power;
private float gyroSpeed;
private float gyroAngle;
private int motorPos;
private int motorSpeed;
private float tInterval = 0.01f;
private float globalAngle;
public SejwayGyro()
{
gyro = new GyroSensor(SensorPort.S2);
}
public void getBalancePos()
{
LCD.drawString("Put down NXT", 0, 0);
Button.waitForAnyPress();
LCD.drawString("Calibrating...", 0, 0);
gyro.recalibrateOffset();
MotorPort.B.resetTachoCount();
MotorPort.C.resetTachoCount();
LCD.drawString("Balance NXT", 0, 0);
Button.waitForAnyPress();
}
public void pidControl()
{
while (!Button.ESCAPE.isDown())
{
updateGyroData(gyro);
updateMotorData();
power = KGYROSPEED * gyroSpeed;
//KGYROANGLE * gyroAngle;
//KPOS * motorPos +
//KSPEED * motorSpeed;
MotorPort.B.controlMotor((int)power, BasicMotorPort.FORWARD);
MotorPort.C.controlMotor((int)power, BasicMotorPort.FORWARD);
LCD.clear();
LCD.drawString("gyroSpeed:", 0, 1);
LCD.drawString("gyroAngle:", 0, 2);
LCD.drawString("motorPos:", 0, 3);
LCD.drawString("motorSpeed:", 0, 4);
LCD.drawString("power:", 0, 5);
LCD.drawInt((int)gyroSpeed, 10, 1);
LCD.drawInt((int)gyroAngle, 4, 10, 2);
LCD.drawInt(motorPos, 4, 10, 3);
LCD.drawInt(motorSpeed, 4, 10, 4);
LCD.drawInt((int)power, 4, 10, 5);
LCD.refresh();
Delay.msDelay(10);
}
}
private void updateMotorData() {
motorPos = Motor.B.getTachoCount();
motorSpeed = Motor.B.getRotationSpeed();
}
private void updateGyroData(GyroSensor gyro) {
gyroSpeed = gyro.getAngularVelocity();
globalAngle += gyroSpeed * tInterval;
gyroAngle = globalAngle;
}
public static void main(String[] args)
{
SejwayGyro sej = new SejwayGyro();
sej.getBalancePos();
sej.pidControl();
}
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment