out[pixPtr++] = 0xff000000 + (B <<>
}
}
}
public void surfaceCreated(SurfaceHolder holder) {
//The Surface has been created, acquire the camera and tell it where to draw
mCamera = Camera.open();
try {
mCamera.setDisplayOrientation(90);
mCamera.setPreviewDisplay(holder);
mCamera.setPreviewCallback(this);
} catch (IOException exception) {
mCamera.release();
mCamera = null;
// TODO: add more exception handling logic here
}
}
public void surfaceDestroyed(SurfaceHolder holder) {
//Surface will be destroyed when we return, so stop the preview.
//Because the CameraDevice object is not a shared resource, it's very
//important to release it when the activity is paused
mCamera.stopPreview();
mCamera.release();
mCamera = null;
}
public void surfaceChanged(SurfaceHolder holder, int format, int w, int h) {
//Now that the size is known, set up the camera parameters and begin the preview
Camera.Parameters parameters = mCamera.getParameters();
parameters.setPreviewSize(this.w, this.h);
parameters.setPreviewFormat(ImageFormat.JPEG);
parameters.setSceneMode("action");
// parameters.setFlashMode(Camera.Parameters.FLASH_MODE_TORCH);
mCamera.setParameters(parameters);
mCamera.startPreview();
}
@Override
public void onPreviewFrame(byte[] data, Camera camera) {
previewBuffer = data;
}
@Override
protected void onDetachedFromWindow() {
super.onDetachedFromWindow();
mHolder.removeCallback(this);
}
}
Fra denne kode er det tråden t som er mest interessant, da det er denne som foretager det nødvendige beregninger og sender dem over bluetooth ved hjælp af følgende klasse:
package jan.lego;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.UUID;
import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
import android.bluetooth.BluetoothSocket;
public class BluetoothForNXT {
private BluetoothAdapter mBluetoothAdapter = null;
private BluetoothSocket btSocket = null;
private DataOutputStream outStream = null;
private static final UUID MY_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
private static String address = "00:16:53:02:A8:D2"; //hardcoded MAC for "bricksorter"
public BluetoothForNXT() {
mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter();
connect();
}
private boolean connect() {
if (mBluetoothAdapter == null) {
return false;
}
if (!mBluetoothAdapter.isEnabled()) {
return false;
}
BluetoothDevice device = mBluetoothAdapter.getRemoteDevice(address);
try {
btSocket = device.createRfcommSocketToServiceRecord(MY_UUID);
} catch (IOException e) {
return false;
}
mBluetoothAdapter.cancelDiscovery();
try {
btSocket.connect();
} catch (IOException e) {
try {
btSocket.close();
} catch (IOException e2) {
return false;
}
}
return true;
}
public boolean send(String message) {
try {
outStream = new DataOutputStream(btSocket.getOutputStream());
} catch (IOException e) {
return false;
}
message += "\n";
try {
outStream.writeBytes(message);
return true;
} catch (IOException e) {
return false;
}
}
}
Denne klasse forbinder blot til NXT'en, holder forbindelsen åben, for derefter at sende beskeder i det førnævnte format. Den vigtigste klasse i android delen er:
package jan.lego;
import java.util.ArrayList;
import android.util.Log;
public class CameraSensor {
private int w = 176;
private int h = 144;
/**
* unknown, red, green = 0, 1, 2
*
* @param colors
* @return
*/
public ArrayList getColorPositions(ArrayList colors) {
ArrayList colorPositions = new ArrayList();
for(int i = 0; i < i =" i+3)">
int r = colors.get(i);
int g = colors.get(i+1);
int b = colors.get(i+2);
if(r > 140 && g < 100 && b <100) { //red
colorPositions.add(1);
continue;
}
if(r < 100 && g > r) { //green
colorPositions.add(2);
continue;
}
colorPositions.add(0);
}
public int getColor(ArrayList colorEnums) {
int largestLength = 0;
int largestPos = 0;
int largestColor = 0;
int start = 0;
int prevColor = 0;
int colorLength = 0;
for(int pos=0; pos
int currentColor = colorEnums.get(pos);
if(currentColor == 1 || currentColor == 2) { //red or green?
if(currentColor == prevColor) { //same color
colorLength++;
} else { //different color
colorLength = 1;
start = pos;
}
}
prevColor = currentColor;
//largest
if(colorLength > largestLength) {
largestLength = colorLength;
largestPos = start;
largestColor = currentColor;
}
}
/*
* largest return
*/
if(largestLength > Math.ceil(h/2)+40) {
return largestColor;
}
return 0;
}
public int getHits(ArrayList colorEnums) {
int res = 0;
for(int pos=0; pos
int currentColor = colorEnums.get(pos);
if(currentColor == 1 || currentColor == 2) { //red or green?
res++;
}
}
return res;
}
}
Denne klasse indeholder metoden getColorPositions(ArrayList colors), som tager en liste af farver for hver pixel fra kameraet, løber dette igennem, for derefter at returnere et array med en længde svarende til antallet af pixels, med et tal mellem 0 og 2 på hver plads. Her er 0 igen en ukendt farve, 1 rød og 2 grøn. Farverne gendkendes i følgende if-sætninger i metoden:
int r = colors.get(i);
int g = colors.get(i+1);
int b = colors.get(i+2);
if(r > 140 && g < 100 && b <100) { //red
colorPositions.add(1);
continue;
}
if(r < 100 && g > r) { //green
colorPositions.add(2);
continue;
}
colorPositions.add(0);
Farven rød bestemmes dermed f.eks. ud fra hvilken værdi de enkelte farve nuancer i den enkelte pixel har. Hvis den røde farve nuance er over 140 og den grønne og blå er under 100, vil dette svare til rød. Hvis der havde været mere tid ville det have været en fordel med en eller anden form for kalibrering af farverne, for på den måde at tage højde for forskellige lys forhold. F.eks. oplevede vi da vi sad og testede mobilen uden robotten og havde fået denne til at genkende farver, så virkede farve genkendelsen ikke længere når mobilen sad på robotten, da robottens skygge ændrede farverne. Til at løse dette problem prøvede vi at benytte os af en LED, dog havde vi ikke flere motor porte, som en led kræver, og telefonens indbyggede var alt for kraftig. Løsningen blev at finde de værdier der passede til når den sad på robotten.
Android sender som sagt til NXT'en over bluetooth. Koden på NXT'en ser ud som følger:
package Robot;
import java.util.ArrayList;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
public class CameraSensor {
String data = "";
BTReceiver btr;
ArrayList tdata = new ArrayList();
int brickColor = 0;
public CameraSensor(){
btr = new BTReceiver(this);
Thread t = new Thread(btr);
t.start();
}
public int getBrickColor() {
if(Claw.isOpen())
brickColor = tdata.get(1);
return brickColor;
}
public int getHits() {
return tdata.get(0);
}
public void setData(String data) {
this.data = data;
if(data.equals("")) return;
tdata = splitData();
}
private ArrayList splitData(){
ArrayList res = new ArrayList();
try {
int splitter = data.indexOf(';');
res.add(Integer.parseInt(data.substring(0,splitter)));
res.add(Integer.parseInt(data.substring(splitter+1, splitter+2)));
} catch(Exception e) {
}
return res;
}
}
Denne klasse opretter en tråd som opdatere en felt variabel ved navn data. Det er dermed trådens job altid at sørge for data er "frisk". Data bruges nu til at hente farven på klodsen og antal hits. For at få dette data var det nødvendigt at implementere en simpel streng splitter metode, da String.split() ikke er implementeret i LeJOS.
CameraSensor klassen benyttes i følgende opførsel:
package BrickSorter;
import Robot.CameraSensor;
import Robot.Claw;
import Robot.LightSensorNavigator;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.robotics.subsumption.Behavior;
public class FindBrick implements Behavior {
private boolean _suppressed = false;
private CameraSensor cs;
private LightSensorNavigator lsn;
public FindBrick(CameraSensor cs, LightSensorNavigator lsn) {
this.cs = cs;
this.lsn = lsn;
}
@Override
public void action() {
LCD.drawString("color: "+cs.getBrickColor(), 0, 1);
grab();
}
@Override
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
@Override
public boolean takeControl() {
return Claw.isOpen();
}
private void grab() {
if(cs.getHits() > 130){
Claw.close();
lsn.reset();
}
}
}
Her ses det at kloen f.eks. er sat til gribe om en klods, hvis den modtager 130 hits eller mere. Yderligere gælder denne opførsel kun hvis kloen er åben. Når kloen lukker omkring en klods vil en anden opførsel tage over (GreyNavigator).
Farvemålinger:
Vi havde problemer med at kende forskel på rød og grøn, da vores robot så grøn, når den stod lige imellem den sorte og den røde linie. Derfor valgte vi at lave en test, hvor vi fik værdierne for en enkelt lyssensor skrevet ned i en log, mens den kørte fra det sorte og hen over de røde striber eller de grønne striber. Disse værdier lavede vi derefter om til en graf for at se om forskellen mellem rød og grøn simpelthen var for lille.
Den røde streg repræsenterer målingerne, hvor NXT'en kører hen over de røde striber og ind i målzonen og den grønne for de grønne striber. Vi startede ca. midt på banen, så den lyseste del af banen er ikke med på grafen.
Det kan ses af værdierne falder, når robotten bevæger sig fra de lyse farver mod den sorte del af banen, og vi får et stor udslag når striberne rammes. Det er derfor tydeligt at vi burde kunne se at der er sket et skift fra sort til enten rød eller grøn. Forskellen mellem de røde udslag og grønne udslag er også så store at vi burde kunne kende forskel mellem dem.
Den sidste stribe er så tynd at den ikke når at give samme udslag som de andre, men da vi kun benytter den yderste i vores endelige projekt, har dette ingen betydning.
I vores tidligere kode vurderede farven allerede da den så et lille udsving og derfor kunne rød blive vurderet til grøn. Vi måtte derfor ændre på denne vurdering.
Vi besluttede os derfor for først at vurdere en farve når sensorværdien havde været i dens maks og var begyndt at falde igen. Her udnyttede vi at vi havde lavet en cache på lyssensorene og vi kunne derfor se om den nuværende værdi var lavere end den foregående. Når dette indtraf kigger vi på den minimale værdi og maksimale værdi i cachen og vurderer om udslaget er stort nok. Hvis det er, så har vi set en farve og skal derfor vurderer farven ud fra den maksimale værdi.
Vi inddeler målingerne i tre niveauer, hvor det øverste niveau giver rød og udregnes ved at lægge værdien for rød og grøn sammen i vores kalibrering og dividere med to. Dette giver os en linie der skiller rød og grøn (T1). Skillelinien mellem sort og grøn udregnes ved at trække en på forhånd valgt værdi (som vi vurderede til at være 40) fra værdien for grøn(T2). Sammenlagt giver dette os to linier som indeler værdierne i tre niveauer, så vi kan vurdere hvilken farve vi kan se.
Vi lavede også målinger der gik fra den lyse del af banen til den mørke, for at se hvorfor vores GreyNavigtor havde så store problemer med at finde den korrekte retning når robotten befandt sig i den lyse ende. Graf for denne måling:
Vi kan ud fra disse målinger se at den lyse del af banen har mindre fald i værdierne og nogen store udsving, som gør det svært for GreyNavigator at vende robotten den rigtige vej. Derfor er denne del af banen ikke brugbar i vores fremvisning i morgen.
FindBrick:
Denne adfærd skulle have været den der udnyttede kamerasensoren til at finde en brik.
Kode:
package BrickSorter;
import Robot.CameraSensor;
import Robot.Claw;
import Robot.LightSensorNavigator;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.robotics.subsumption.Behavior;
public class FindBrick implements Behavior {
private boolean _suppressed = false;
private CameraSensor cs;
private LightSensorNavigator lsn;
public FindBrick(CameraSensor cs, LightSensorNavigator lsn) {
this.cs = cs;
this.lsn = lsn;
}
@Override
public void action() {
LCD.drawString("color: "+cs.getBrickColor(), 0, 1);
grab();
}
@Override
public void suppress() {
_suppressed = true;
}
@Override
public boolean takeControl() {
return Claw.isOpen();
}
private void grab() {
if(cs.getHits() > 130){
Claw.close();
lsn.reset();
}
}
}
Denne adfærd tager kontrollen når kloen er åben. I action bliver metoden grab kaldt, som tjekker om der er nok hits, til at der rent faktisk er en klods imellem kløerne på den. Hvis dette er tilfældet, lukkes kloen.
Dette betyder at vores FindBrick skal "fodres" med klodserne fremfor selv at bruge kameraet til at finde dem.
ColorNavigator:
Vi besluttede os for at lave store ændringer i ColorNavigator og helt fjerne FinishLine, da vi var presset med tid og vi skulle have en virkende robot.
Koden for den nye ColorNavigator:
package BrickSorter;
import Robot.CameraSensor;
import Robot.Claw;
import Robot.LightSensorNavigator;
import Robot.Movement;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.robotics.subsumption.Behavior;
public class ColorNavigator implements Behavior{
private LightSensorNavigator lsn;
private boolean _suppressed = false;
private int actionTime = 20;
private int diffLevel = 30;
private CameraSensor cs;
private int reverseTime = 3000;
private int reverseSpeed = 30;
private int anglingTime = 1000;
private int anglingSpeed = 30;
private boolean rightSeenColor = false;
private boolean leftSeenColor = false;
public ColorNavigator(LightSensorNavigator lsn, CameraSensor cs) {
this.lsn = lsn;
this.cs = cs;
}
@Override
public void action() {
//Is the color red or green
lsn.setHasSeenRing(true);
Movement.stop();
Sound.pause(500);
int rightColor = lsn.getRightColor();
int leftColor = lsn.getLeftColor();
if(rightSeenColor){
Sound.beep();
while(lsn.getLeftColor() != rightColor){
Movement.drive(20, -5);
Sound.pause(actionTime);
}
if(cs.getBrickColor() != rightColor){
reverseAway(rightColor);
return;
}
Movement.stop();
Sound.pause(100);
}
else{
Sound.twoBeeps();
while(lsn.getRightColor() != leftColor){
Movement.drive(-5, 20);
Sound.pause(actionTime);
}
if(cs.getBrickColor() != leftColor){
reverseAway(leftColor);
return;
}
Movement.stop();
Sound.pause(100);
}
deliverBrick();
}
private void reverseAway(int color) {
if(color == 1){
Movement.drive(-25, -25);
Sound.pause(4000);
Movement.drive(-30, 30);
Sound.pause(1000);
Movement.drive(30, 30);
Sound.pause(2000);
Movement.stop();
}
else{
Movement.drive(-50, -50);
Sound.pause(2000);
Movement.drive(30, -30);
Sound.pause(1000);
Movement.drive(30, 30);
Sound.pause(2000);
Movement.stop();
}
lsn.reset();
lsn.setHasSeenRing(false);
}
private void deliverBrick(){
Movement.drive(30,30);
Sound.pause(3000);
Movement.stop();
Claw.open();
Movement.drive(-100, -100);
Sound.pause(2000);
lsn.reset();
Movement.stop();
}
private void reverse(int left, int right){
Movement.drive(-left, -right);
Sound.pause(anglingTime);
Movement.drive(-reverseSpeed, -reverseSpeed);
Sound.pause(reverseTime);
lsn.reset();
lsn.setHasSeenRing(false);
}
@Override
public void suppress() {
_suppressed = true;// standard practice for suppress methods
}
@Override
public boolean takeControl() {
int rightDiff = lsn.getRightDiff();
int leftDiff = lsn.getLeftDiff();
rightSeenColor = rightDiff > diffLevel && lsn.checkRightPeak();
leftSeenColor = leftDiff > diffLevel && lsn.checkLeftPeak();
return (rightSeenColor) || (leftSeenColor);
}
}
Efter de nye farvemålinger har vi nu lavet om på takeControl til stadig at kigge på differencen mellem den nye måling og den foregående, men også tjekke efter en nedgang i værdi, som indikerer at sensoren er flyttet over stregen og er på vej ud i det sorte område igen.
Den største ændring i action-metoden er at den nu ikke lader FinishLine komme til, men istedet kører metoden deliverBrick, som er en sekvens af handlinger, der sender robotten fra den yderste ring, ind i målzonen, åbner kloen og bakker ud igen. Da robotten altid retter op på den farvede stribe, vil dette resultere i at robotten altid vil ramme midten af målzonen.
Hvis stribens farve ikke er den samme som klodsens, bruger robotten metoden reverseAway, som bakker væk fra striben og kører over mod den farve som klodsen har. Herefter overtager GreyNavigator igen, da robotten gerne skulle være ca. midt på banen, og den vil nu bevæge sig ned mod målzonerne igen, og forhåbentlig ramme den korrekte farve.
Adfærdshierarki:
Det endelige hierarki:
Vi har fjernet FinishLine da vi ikke kunne nå at få GreyNavigator og ColorNavigator til at samarbejde om at få robotten helt ind til målzonen. Derfor står ColorNavigator for at få den fra den yderste stribe til målzonen.
Resultat:
Mobiltelefonen kan nu sende data til NXT'en som den kan bruge til at vurdere om der er en klods foran den og hvilken farve den har.
Vores forsøg med banen bekræfter at vi kan kende forskel på sort, rød og grøn og dette virker også i vores kode.
Vi har nu det endelige adfærdshierarki, som kan få robotten til at tage en brik, kører i mål, og kører ud igen, for derefter at være klar til at modtage en klods mere.
