Commit 2dad0faf authored by Casper's avatar Casper
Browse files

added color sensor segway

parent 3d6f024f
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.*;
public class SeqwayColor {
// Global vars:
int offset;
int prev_error;
float int_error;
ColorSensor cs;
static DataLogger dl;
// PID constants
final int KP = 45;
final int KI = 0;
final int KD = 1;
final int SCALE = 1;
public SeqwayColor()
{
cs = new ColorSensor(SensorPort.S3);
}
public void getBalancePos()
{
// Wait for user to balance and press orange button
while (!Button.ENTER.isDown())
{
// NXTway must be balanced.
offset = cs.getNormalizedLightValue();
LCD.clear();
LCD.drawInt(offset, 2, 4);
LCD.refresh();
}
}
public void pidControl()
{
while (!Button.ESCAPE.isDown())
{
int normVal = cs.getNormalizedLightValue();
// Proportional Error:
int error = normVal - offset;
// Integral Error:
int_error = int_error + error;
// 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
// dl.writeSample(error);
// dl.writeSample((int)int_error);
// dl.writeSample(deriv_error);
// dl.writeSample(pid_val);
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);
}
}
}
public void shutDown()
{
// Shut down light sensor, motors
Motor.B.flt();
Motor.C.flt();
cs.setFloodlight(false);
// dl.close();
}
public static void main(String args[]) {
// dl = new DataLogger("data.txt");
// dl.start();
SeqwayColor sw = new SeqwayColor();
sw.getBalancePos();
sw.pidControl();
sw.shutDown();
}
}
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