Commit 76252d24 authored by Bernd Menia's avatar Bernd Menia
Browse files

One small step for Günther, but one big step for RoBro.

parent 1ec22872
Pipeline #12135 skipped
......@@ -47,7 +47,7 @@ public class BeaconDetectionActivity extends Activity implements CameraBridgeVie
public static ArrayList<Blob> colorBlobList = new ArrayList<>();
public ArrayList<Beacon> beaconList = new ArrayList<>();
public Odometry odometry = new Odometry();
public SelfLocalization selfLocalization = new SelfLocalization();
/** convertHSVtoPseudo
......@@ -208,7 +208,7 @@ public class BeaconDetectionActivity extends Activity implements CameraBridgeVie
Utilities.showLog("blobs in colorBlobList: " + colorBlobList.toString());
colorBlobList.clear();
Utilities.showLog("beacon list: " + beaconList.toString());
odometry.selfLocalize(beaconList, beaconCoordinates);
selfLocalization.selfLocalize(beaconList, beaconCoordinates);
beaconList.clear();
return mRgba;
}
......
......@@ -270,10 +270,6 @@ public class ColorBlobDetectionActivity extends Activity implements OnTouchListe
Imgproc.resize(mDetector.getSpectrum(), mSpectrum, SPECTRUM_SIZE);
lowestPoint = mDetector.getLowestPoint(0);
Imgproc.circle(mRgba, lowestPoint, 50, new Scalar(0, 0, 255), 1, 8,0);
Point norbert = new Point(0, 0);
norbert = HomographyActivity.convertPoint(MainActivity.mHomo, norbert);
Utilities.showLog("norbert: " + norbert.toString());
}
/** toggleOnClick
......
......@@ -347,10 +347,10 @@ public class MovementActivity extends Activity {
e.printStackTrace();
}
}
Odometry.updatePosition(ticksLeftWheel, ticksRightWheel);
SelfLocalization.updatePosition(ticksLeftWheel, ticksRightWheel);
*/
}
// Odometry.updatePosition(ticksLeftWheel, ticksRightWheel);
// SelfLocalization.updatePosition(ticksLeftWheel, ticksRightWheel);
roBro.readWrite("s");
}
......
package com.example.lena.helloworld;
import android.content.Context;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.concurrent.Executor;
import java.util.concurrent.Executors;
/**
* Created by root on 25.06.16.
*/
......@@ -22,15 +22,9 @@ public class ObstacleAvoidance implements Runnable {
private int turnedLeft;
public ObstacleAvoidance() {
movementActivity = MovementActivity.getInstance();
}
// TODO needed?
public ObstacleAvoidance(Context context) {
movementActivity = MovementActivity.getInstance();
}
/* CONSTRUCOTR */
public ObstacleAvoidance(ArrayBlockingQueue blockingQueueMovementActivity) {
movementActivity = MovementActivity.getInstance();
this.blockingQueueMovementActivity = blockingQueueMovementActivity;
......@@ -41,6 +35,7 @@ public class ObstacleAvoidance implements Runnable {
/* FURTHER METHODS */
public synchronized void run() {
//int randomNum = rand.nextInt((max - min) + 1) + min
......@@ -51,7 +46,6 @@ public class ObstacleAvoidance implements Runnable {
this.turnedLeft = 1;
}
//turnedLeft = -1;
// TODO Turn right also!
calibrateToObstacle();
......@@ -66,14 +60,12 @@ public class ObstacleAvoidance implements Runnable {
movementActivity.update();
this.waitingQueue.add(new Object());
//while ((distances = movementActivity.queryDistanceSafe()).length < 5) {
//}
try {
switch (blockingQueueObstacleAvoidance.take()) {
case BOTH:
Utilities.showLog("Update: First Obstacle BOTH");
continue;
case OBSTACLE:
movementActivity.forward();
Utilities.showLog("Update: First Obstacle OBSTACLE");
......@@ -83,6 +75,7 @@ public class ObstacleAvoidance implements Runnable {
case OTHER:
Utilities.showLog("Update: First Obstacle OTHER");
break;
case STOP:
movementActivity.stop();
Utilities.showLog("Update: First Obstacle Loop STOP");
......@@ -94,7 +87,6 @@ public class ObstacleAvoidance implements Runnable {
movementActivity.update();
//movementActivity.forward(2800);
movementActivity.forward(4000);
Utilities.showLog("Update: between loops forward");
movementActivity.update();
......@@ -116,31 +108,16 @@ public class ObstacleAvoidance implements Runnable {
}
}
/*
try {
do {
while ((distances = movementActivity.queryDistanceSafe()).length < 5) {
}
movementActivity.forward();
} while ((blockingQueueObstacleAvoidance.take()) == ObstacleAvoidanceEnum.BOTH);
} catch (InterruptedException e) {
e.printStackTrace();
}
*/
executor.execute(new CheckObstacleWalls(blockingQueueObstacleAvoidance, this.waitingQueue, turnedLeft==1));
executor.execute(new CheckObstacleWalls(blockingQueueObstacleAvoidance, this.waitingQueue, turnedLeft==1));
/* Move along the side of the obstacle until the robot has passed it completely. */
while (true) {
Utilities.showLog("Update: SecondLoop Obstacle");
movementActivity.update();
this.waitingQueue.add(new Object());
//while ((distances = movementActivity.queryDistanceSafe()).length < 5) {
//}
try {
switch (blockingQueueObstacleAvoidance.take()) {
case BOTH:
......@@ -176,43 +153,11 @@ public class ObstacleAvoidance implements Runnable {
}
}
//MovementActivity.state = State.FORWARD;
blockingQueueMovementActivity.add(State.FORWARD);
}
/*
public synchronized void runOLD() {
calibrateToObstacle();
movementActivity.turn(90);
while (true) {
while ((distances = movementActivity.queryDistanceSafe()).length < 5) {
}
while (checkObstacle(30)) {
//movementActivity.stop();
movementActivity.forward(3000);
}
//movementActivity.stop();
movementActivity.turnRight(90);
while (checkObstacle(30)) {
//movementActivity.stop();
movementActivity.forward(3000);
}
//movementActivity.stop();
blockingQueueMovementActivity.add(State.FORWARD);
break;
}
}
*/
private void calibrateToObstacle() {
while (true) {
......@@ -233,22 +178,4 @@ public class ObstacleAvoidance implements Runnable {
}
}
}
// TODO OUT OF DATE!
private boolean checkObstacle(int angle) {
//movementActivity.stop();
movementActivity.turn(-angle);
Utilities.showLog("Checking Obstacle...");
if ((distances = movementActivity.queryDistanceSafe())[4] > 400) {
movementActivity.turn(angle);
return true;
}
movementActivity.turn(angle);
return false;
}
}
}
\ No newline at end of file
......@@ -91,13 +91,13 @@ public class RobotWhisperer{
try {
Thread.sleep(100);
} catch (InterruptedException e) {
//Utilities.showLog("herpes");
e.printStackTrace();
}
physicaloid.setConfig(uartConfig);
try {
Thread.sleep(100);
} catch (InterruptedException e) {
//Utilities.showLog("syphillis");
e.printStackTrace();
}
physicaloid.open(uartConfig);
......
......@@ -16,16 +16,15 @@ import java.util.ArrayList;
import java.util.HashMap;
public class Odometry {
/* ATTRIBUTES */
/* Warning, these are only estimated values. They are not totally accurate. */
private static final double CENTIMETERS_PER_TICK = 0.0249;
private static final double WHEEL_CIRCUMFERENCE = 56;
public class SelfLocalization {
/* ATTRIBUTES */
private Thread t;
Point[] lowestPoint = new Point[2];
/** selfLocalize
* creates a circle around the beacons and detects where they are intersecting each other
* @param beaconList
......@@ -95,7 +94,7 @@ public class Odometry {
//double theta = beaconWorldAngle + beaconEgocentricAngle;
//System.out.println("beaconWorldAngle: " + beaconWorldAngle);
System.out.println("beaconEgocentricAngle: " + beaconEgocentricAngle);
//System.out.println("Current odometry: x: " + p.x + " y: " + p.y + " theta: " + theta);
//System.out.println("Current selfLocalization: x: " + p.x + " y: " + p.y + " theta: " + theta);
Utilities.showLog("Beacon 1 coordinates: " + lowestPoint[0]);
Utilities.showLog("Beacon 2 coordinates: " + lowestPoint[1]);
} catch (Exception e) {
......@@ -107,44 +106,4 @@ public class Odometry {
Utilities.showLog("only " + beaconList.size() + " beacons detected");
}
}
// TODO only works for a left rotation.
public static void updatePosition(int ticksLeftWheel, int ticksRightWheel) {
double angle;
boolean rotateCounterClockWise = true;
int ticksDriven;
ticksDriven = Math.abs(ticksLeftWheel-ticksRightWheel);
if (ticksLeftWheel < ticksRightWheel) {
angle = caluclateCircularAngle(Math.abs(ticksRightWheel - ticksLeftWheel));
}
else if (ticksLeftWheel > ticksRightWheel) {
angle = caluclateCircularAngle(Math.abs(ticksLeftWheel - ticksRightWheel));
rotateCounterClockWise = false;
}
else {
angle = 0;
}
Utilities.addPointToRobotPosition(calculatePointToAdd(angle, ticksDriven));
Utilities.angleRobot = (Utilities.angleRobot + angle) % 360;
}
private static Point calculatePointToAdd(double angle, int ticksDriven) {
return new Point(Math.cos(angle) * (ticksDriven * CENTIMETERS_PER_TICK),
Math.sin(angle) * (ticksDriven * CENTIMETERS_PER_TICK));
}
private static double caluclateCircularAngle(int ticks) {
return ((ticks * CENTIMETERS_PER_TICK) / WHEEL_CIRCUMFERENCE) * 360;
}
}
\ No newline at end of file
Markdown is supported
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