Skip to content

GitLab

Commit 3f268dbf authored by Peder Kronsgaard Detlefsen's avatar Peder Kronsgaard Detlefsen
Browse files

Added Car, PCpidController, Sejway and SejwayControl.

parent 2dad0faf
Hide whitespace changes
Inline Side-by-side
lesson5/Car.java 0 → 100644
View file @ 3f268dbf
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);
}
}
lesson5/PCpidController.java 0 → 100644
View file @ 3f268dbf
import java.awt.BorderLayout;
import java.awt.GridLayout;
import java.awt.TextField;
import java.awt.event.*;
import javax.swing.JButton;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.JLabel;
import lejos.pc.comm.*;
import java.io.*;
/**
* A GUI that makes it possible to establish a Bluetooth connection
* to an NXT controlled car. The car is then controlled by a power
* and duration value sent to the car. The tacho counter value is
* received and displayed after each command to the car has been performed.
*
* @author Ole Caprani 24.2.12
*
*
*/
public class PCpidController extends JFrame implements ActionListener {
/**
*
*/
private static final long serialVersionUID = -2481242253095448939L;
private TextField nameField = new TextField(12);
private TextField addressField = new TextField(20);
private String name = "NXT8";
private String address = "00165310C79D";
private TextField kpField = new TextField(10);
private TextField kiField = new TextField(10);
private TextField kdField = new TextField(10);
private TextField scaleField = new TextField(10);
private TextField valueField = new TextField(10);
private TextField newOffsetField = new TextField(10);
private TextField pidField = new TextField(10);
private NXTComm nxtComm;
private NXTInfo nxtInfo;
private InputStream is;
private OutputStream os;
private DataInputStream dis;
private DataOutputStream dos;
private JButton connectButton = new JButton("Connect");
private JButton setButton = new JButton("Set");
/**
* Constructor builds GUI
*/
public PCpidController() {
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
setTitle("Control NXT");
setSize(600,300);
// holds labels and text fields
JPanel p1 = new JPanel();
p1.add(new JLabel("Name:"));
p1.add(nameField);
nameField.setText(name);
p1.add(new JLabel("Address:"));
p1.add(addressField);
addressField.setText(address);
try {
nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
} catch (NXTCommException nce) {
}
nxtInfo = new NXTInfo();
// holds connect button
JPanel p2 = new JPanel();
p2.add(connectButton);
connectButton.addActionListener(this);
// holds labels and text fields
JPanel p3 = new JPanel();
p3.add(new JLabel("KP:"));
p3.add(kpField);
kpField.setText("28");
p3.add(new JLabel("KI:"));
p3.add(kiField);
kiField.setText("4");
p3.add(new JLabel("KD:"));
p3.add(kdField);
kdField.setText("33");
p3.add(new JLabel("Scale:"));
p3.add(scaleField);
scaleField.setText("18");
// hold new offset field
JPanel p6 = new JPanel();
p6.add(new JLabel("value:"));
valueField.setEditable(false);
p6.add(valueField);
p6.add(new JLabel("new offset:"));
p6.add(newOffsetField);
// holds go button
JPanel p4 = new JPanel();
p4.add(setButton);
setButton.addActionListener(this);
// holds PID value field.
JPanel p5 = new JPanel();
p5.add(new JLabel("pid_val:"));
pidField.setEditable(false);
p5.add(pidField);
// North area of the frame
JPanel panel = new JPanel();
panel.setLayout(new GridLayout(7,1));
panel.add(p1);
panel.add(p2);
panel.add(p3);
panel.add(p6);
panel.add(p4);
panel.add(p5);
add(panel,BorderLayout.NORTH);
}
/**
* Required by action listener;
* only action is generated by the get Length button
*/
public void actionPerformed(ActionEvent e) {
if(e.getSource()== connectButton) {
String name = nameField.getText();
String address = addressField.getText();
nxtInfo.name = name;
nxtInfo.deviceAddress = address;
try {
nxtComm.open(nxtInfo);
is = nxtComm.getInputStream();
os = nxtComm.getOutputStream();
dis = new DataInputStream(is);
dos = new DataOutputStream(os);
} catch (Exception ex) {
}
}
if(e.getSource() == setButton) {
try {
String kpString = kpField.getText();
int kp = new Integer(kpString).intValue();
dos.writeInt(kp);
String kiString = kiField.getText();
int ki = new Integer(kiString).intValue();
dos.writeInt(ki);
String kdString = kdField.getText();
int kd = new Integer(kdString).intValue();
dos.writeInt(kd);
String scaleString = scaleField.getText();
int scale = new Integer(scaleString).intValue();
dos.writeInt(scale);
String newOffsetString = newOffsetField.getText();
int newOffset = new Integer(newOffsetString).intValue();
dos.writeInt(newOffset);
dos.flush();
} catch (Exception ex) {
}
}
}
/**
* Initialize the display Frame
*/
public static void main(String[] args) {
PCpidController frame = new PCpidController();
frame.setVisible(true);
// Read PID and light value and update display.
while (true) frame.refreshValues();
}
private void refreshValues() {
try {
int pid = dis.readInt();
pidField.setText("" + pid);
int value = dis.readInt();
valueField.setText("" + value);
} catch (Exception e) {
}
}
}
\ No newline at end of file
lesson5/Sejway.java 0 → 100644
View file @ 3f268dbf
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
{
// PID constants
final int KP = 28;
final int KI = 4;
final int KD = 33;
final int SCALE = 18;
// Global vars:
int offset;
int prev_error;
float int_error;
LightSensor ls;
public Sejway()
{
ls = new LightSensor(SensorPort.S2, 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 pidControl()
{
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);
}
}
}
public void shutDown()
{
// Shut down light sensor, motors
Motor.B.flt();
Motor.C.flt();
ls.setFloodlight(false);
}
public static void main(String[] args)
{
Sejway sej = new Sejway();
sej.getBalancePos();
sej.pidControl();
sej.shutDown();
}
}
\ No newline at end of file
lesson5/SejwayControl.java 0 → 100644
View file @ 3f268dbf
import java.io.DataInputStream;
import java.io.DataOutputStream;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
/**
* 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 SejwayControl
{
private String connected = "Connected";
private String waiting = "Waiting...";
private String closing = "Closing...";
private BTConnection btc;
private DataInputStream dis;
private DataOutputStream dos;
// PID constants
private int KP = 28;
private int KI = 4;
private int KD = 33;
private int SCALE = 18;
// Global vars:
int offset;
int prev_error;
float int_error;
LightSensor ls;
public SejwayControl()
{
ls = new LightSensor(SensorPort.S2, true);
LCD.drawString(waiting,0,0);
btc = Bluetooth.waitForConnection();
LCD.clear();
LCD.drawString(connected,0,0);
dis = btc.openDataInputStream();
dos = btc.openDataOutputStream();
}
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.drawString("Offset:", 0, 1);
LCD.drawInt(offset, 10, 1);
LCD.drawString("KP:", 0, 2);
LCD.drawString("KI:", 0, 3);
LCD.drawString("KD:", 0, 4);
LCD.drawString("SCALE:", 0, 5);
LCD.drawInt(KP, 4, 10, 2);
LCD.drawInt(KI, 4, 10, 3);
LCD.drawInt(KD, 4, 10, 4);
LCD.drawInt(SCALE, 4, 10, 5);
LCD.refresh();
}
try {
dos.writeInt(offset);
dos.flush();
} catch (Exception e) {
}
}
public void pidControl()
{
while (!Button.ESCAPE.isDown())
{
try {
if (dis.available() >= 16) {
KP = dis.readInt();
KI = dis.readInt();
KD = dis.readInt();
SCALE = dis.readInt();
offset = dis.readInt();
LCD.drawInt(offset, 10, 1);
LCD.drawInt(KP, 4, 10, 2);
LCD.drawInt(KI, 4, 10, 3);
LCD.drawInt(KD, 4, 10, 4);
LCD.drawInt(SCALE, 4, 10, 5);
LCD.refresh();
}
} catch (Exception e) {
}
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);
}
try {
dos.writeInt(normVal);
dos.writeInt(pid_val);
dos.flush();
} catch (Exception e) {
}
}
}
public void shutDown()
{
// Shut down light sensor, motors
Motor.B.flt();
Motor.C.flt();
ls.setFloodlight(false);
LCD.clear();
LCD.drawString(closing,0,0);
try {
dis.close();
dos.close();
Thread.sleep(100); // wait for data to drain
btc.close();
} catch (Exception e) {
}
}
public static void main(String[] args)
{
SejwayControl sej = new SejwayControl();
sej.getBalancePos();
sej.pidControl();
sej.shutDown();
}
}
\ 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