Commit b501ee78 authored by Martin Bjerrum Henriksen's avatar Martin Bjerrum Henriksen
Browse files

ALT kode

parent 624ffc9d
Pipeline #2772 skipped
import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;
/**
* Receive data from a PC, a phone,
* or another Bluetooth device.
*
* Waits for a Bluetooth connection, receives two integers that are
* interpreted as power and duration for a forward command to a
* differential driven car. The resulting tacho counter
* value is send to the initiator of the connection.
*
* Based on Lawrie Griffiths BTSend
*
* @author Ole Caprani
* @version 26-2-13
*/
public class BTcontrolledSejway extends Thread
{
private String connected = "Connected";
private String waiting = "Waiting...";
private String closing = "Closing...";
private Sejway sejway;
private boolean running = false;
private BTConnection btc;
private DataInputStream dis;
private DataOutputStream dos;
public BTcontrolledSejway(Sejway s){
this.sejway = s;
}
public void stop(){
running = false;
}
public void run()
{
LCD.drawString(waiting,0,0);
btc = Bluetooth.waitForConnection();
LCD.clear();
LCD.drawString(connected,0,0);
dis = btc.openDataInputStream();
dos = btc.openDataOutputStream();
running = true;
while (running)
{
try
{
int p = dis.readInt();
int i = dis.readInt();
int d = dis.readInt();
int o = dis.readInt();
int dur = dis.readInt();
sejway.setPgain(p);
sejway.setIgain(i);
sejway.setDgain(d);
sejway.setOffset(o);
LCD.drawInt(p,7,0,1);
LCD.drawInt(i,7,0,2);
LCD.drawInt(d,7,0,3);
LCD.refresh();
Thread.sleep(dur);
dos.flush();
}
catch (Exception e)
{}
}
try { //close stuff
btc.close();
dis.close();
dos.close();
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
import lejos.nxt.*;
/**
* A locomotion module with methods to drive
* a car with two independent motors. The left motor
* should be connected to port C and the right motor
* to port B.
*
* @author Ole Caprani
* @version 17.4.08
*/
public class Car
{
// Commands for the motors
private final static int forward = 1,
backward = 2,
stop = 3;
private static MotorPort leftMotor = MotorPort.C;
private static MotorPort rightMotor= MotorPort.B;
private Car()
{
}
public static void stop()
{
leftMotor.controlMotor(0,stop);
rightMotor.controlMotor(0,stop);
}
public static void forward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,forward);
rightMotor.controlMotor(rightPower,forward);
}
public static void backward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,backward);
rightMotor.controlMotor(rightPower,backward);
}
public static int counter()
{
return ( (leftMotor.getTachoCount()+rightMotor.getTachoCount())/2);
}
}
\ No newline at end of file
import lejos.nxt.*;
import java.io.*;
/**
* A simple data logger that can be used to sample a sequence
* of integer data values in a flash file. The
* file is a text consisting of a sequence of lines with
* a time value and a sample data value separated by a comma.
*
* When data has been sampled the flash file can be transfered to a
* PC by means of the tool nxjbrowse.
*
* @author Ole Caprani
* @version 4.02.13
*/
public class DataLogger
{
private File f;
private FileOutputStream fos;
private int startTime;
public DataLogger (String fileName)
{
try
{
f = new File(fileName);
if( ! f.exists() )
{
f.createNewFile();
}
else
{
f.delete();
f.createNewFile();
}
fos = new FileOutputStream(f);
}
catch(IOException e)
{
LCD.drawString(e.getMessage(),0,0);
System.exit(0);
}
startTime = (int)System.currentTimeMillis();
}
public void start()
{
startTime = (int)System.currentTimeMillis();
}
public void writeSample( int time, int sample )
{
try
{
Integer timeInt = new Integer(time);
String timeString = timeInt.toString();
Integer sampleInt = new Integer(sample);
String sampleString = sampleInt.toString();
for(int i=0; i<timeString.length(); i++)
{
fos.write((byte) timeString.charAt(i));
}
// Separate items with comma
fos.write((byte)(','));
for(int i=0; i<sampleString.length(); i++)
{
fos.write((byte) sampleString.charAt(i));
}
// New line
fos.write((byte)('\r'));
fos.write((byte)('\n'));
}
catch(IOException e)
{
LCD.drawString(e.getMessage(),0,0);
System.exit(0);
}
}
public void writeSample( int sample )
{
writeSample((int)System.currentTimeMillis() - startTime, sample);
}
public void close()
{
try
{
fos.close();
}
catch(IOException e)
{
LCD.drawString(e.getMessage(),0,0);
System.exit(0);
}
}
}
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.S4);
DataLogger dl = new DataLogger("gyrosample.txt");
dl.start();
float minValue = 1023, maxValue = 0;
int sampleInterval = 5; // ms
LCD.drawString("Gyro Test:", 0, 0);
Button.waitForAnyPress();
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);
dl.writeSample((int)value);
Thread.sleep(sampleInterval);
}
dl.close();
}
}
\ No newline at end of file
import lejos.nxt.*;
/**
* A controller for a self-balancing Lego robot with a light sensor
* on port 2. The two motors should be connected to port B and C.
*
* Building instructions in Brian Bagnall: Maximum Lego NXTBuilding
* Robots with Java Brains</a>, Chapter 11, 243 - 284
*
* @author Brian Bagnall
* @version 26-2-13 by Ole Caprani for leJOS version 0.9.1
*/
public class Sejway extends Thread
{
// PID constants
int KP = 110;
int KI = 12;
int KD = 40;
int SCALE = 18;
// Global vars:
int offset;
int prev_error;
float int_error;
LightSensor ls;
public Sejway()
{
ls = new LightSensor(SensorPort.S3, true);
}
public void getBalancePos()
{
// Wait for user to balance and press orange button
while (!Button.ENTER.isDown())
{
// NXTway must be balanced.
offset = ls.readNormalizedValue();
LCD.clear();
LCD.drawInt(offset, 2, 4);
LCD.refresh();
}
}
public void run()
{
getBalancePos();
while (!Button.ESCAPE.isDown())
{
int normVal = ls.readNormalizedValue();
// Proportional Error:
int error = normVal - offset;
// Adjust far and near light readings:
if (error < 0) error = (int)(error * 1.8F);
// Integral Error:
int_error = ((int_error + error) * 2)/3;
// Derivative Error:
int deriv_error = error - prev_error;
prev_error = error;
int pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;
if (pid_val > 100)
pid_val = 100;
if (pid_val < -100)
pid_val = -100;
// Power derived from PID value:
int power = Math.abs(pid_val);
power = 55 + (power * 45) / 100; // NORMALIZE POWER
if (pid_val > 0) {
MotorPort.B.controlMotor(power, BasicMotorPort.FORWARD);
MotorPort.C.controlMotor(power, BasicMotorPort.FORWARD);
} else {
MotorPort.B.controlMotor(power, BasicMotorPort.BACKWARD);
MotorPort.C.controlMotor(power, BasicMotorPort.BACKWARD);
}
}
}
void setPgain(int v){KP = v;}
void setIgain(int v){KI = v;}
void setDgain(int v){KD = v;}
void setOffset(int v){offset= v;}
int getPgain(){return KP;}
int getIgain(){return KI;}
int getDgain(){return KD;}
int getOffset(){return offset;}
public void shutDown()
{
// Shut down light sensor, motors
Motor.B.flt();
Motor.C.flt();
ls.setFloodlight(false);
}
}
\ No newline at end of file
import lejos.nxt.Button;
public class Stenmain {
public static void main(String[] args){
Sejway sejway = new Sejway();
BTcontrolledSejway btsejway = new BTcontrolledSejway(sejway);
btsejway.start();
sejway.start();
while(!Button.ESCAPE.isDown()){
try {
Thread.sleep(50);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
btsejway.stop();
}
}
package gyrostuff;
import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;
/**
* Receive data from a PC, a phone,
* or another Bluetooth device.
*
* Waits for a Bluetooth connection, receives two integers that are
* interpreted as power and duration for a forward command to a
* differential driven car. The resulting tacho counter
* value is send to the initiator of the connection.
*
* Based on Lawrie Griffiths BTSend
*
* @author Ole Caprani
* @version 26-2-13
*/
public class BTcontrolledSejway extends Thread
{
private String connected = "Connected";
private String waiting = "Waiting...";
private String closing = "Closing...";
private Sejway sejwayWithDL;
private boolean running = false;
private BTConnection btc;
private DataInputStream dis;
private DataOutputStream dos;
public BTcontrolledSejway(Sejway s){
this.sejwayWithDL = s;
}
public void stop(){
running = false;
}
public void run()
{
LCD.drawString(waiting,0,0);
btc = Bluetooth.waitForConnection();
LCD.clear();
LCD.drawString(connected,0,0);
dis = btc.openDataInputStream();
dos = btc.openDataOutputStream();
running = true;
while (running)
{
try
{
int p = dis.readInt();
int i = dis.readInt();
int d = dis.readInt();
int o = dis.readInt();
int dur = dis.readInt();
sejwayWithDL.setPgain(p);
sejwayWithDL.setIgain(i);
sejwayWithDL.setDgain(d);
sejwayWithDL.setOffset(o);
LCD.drawInt(p,7,0,1);
LCD.drawInt(i,7,0,2);
LCD.drawInt(d,7,0,3);
LCD.refresh();
Thread.sleep(dur);
dos.flush();
}
catch (Exception e)
{}
}
try { //close stuff
btc.close();
dis.close();
dos.close();
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
package gyrostuff;
import lejos.nxt.*;
/**
* A locomotion module with methods to drive
* a car with two independent motors. The left motor
* should be connected to port C and the right motor
* to port B.
*
* @author Ole Caprani
* @version 17.4.08
*/
public class Car
{
// Commands for the motors
private final static int forward = 1,
backward = 2,
stop = 3;
private static MotorPort leftMotor = MotorPort.C;
private static MotorPort rightMotor= MotorPort.B;
private Car()
{
}
public static void stop()
{
leftMotor.controlMotor(0,stop);
rightMotor.controlMotor(0,stop);
}
public static void forward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,forward);
rightMotor.controlMotor(rightPower,forward);
}
public static void backward(int leftPower, int rightPower)
{
leftMotor.controlMotor(leftPower,backward);
rightMotor.controlMotor(rightPower,backward);
}
public static int counter()
{
return ( (leftMotor.getTachoCount()+rightMotor.getTachoCount())/2);
}
}
\ No newline at end of file
package gyrostuff;
import lejos.nxt.*;
import java.io.*;
/**
* A simple data logger that can be used to sample a sequence
* of integer data values in a flash file. The
* file is a text consisting of a sequence of lines with
* a time value and a sample data value separated by a comma.
*
* When data has been sampled the flash file can be transfered to a
* PC by means of the tool nxjbrowse.
*
* @author Ole Caprani
* @version 4.02.13
*/
public class DataLogger
{
private File f;
private FileOutputStream fos;
private int startTime;
public DataLogger (String fileName)
{
try
{
f = new File(fileName);
if( ! f.exists() )
{
f.createNewFile();
}
else
{
f.delete();
f.createNewFile();
}
fos = new FileOutputStream(f);
}
catch(IOException e)
{
LCD.drawString(e.getMessage(),0,0);
System.exit(0);
}
startTime = (int)System.currentTimeMillis();
}
public void start()
{
startTime = (int)System.currentTimeMillis();