Skip to content

GitLab

Commit 53248fdb authored by Julian Zobel's avatar Julian Zobel 🦄
Browse files

More Curves

parent f709146c
Hide whitespace changes
Inline Side-by-side
src/de/tud/kom/p2psim/impl/topology/component/UAVTopologyComponent.java
View file @ 53248fdb
......@@ -281,11 +281,22 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
return state;
}
private PositionVector getBaseReturnLocation() {
PositionVector locationOverBase = baseLocation.clone();
locationOverBase.setAltitude(10);
private PositionVector getBaseHoverLocation() {
PositionVector hoverLocation = baseLocation.clone();
hoverLocation.setAltitude(10);
return hoverLocation;
}
private LinkedList<PositionVector> getBaseReturnRoute() {
PositionVector landingLocation = baseLocation.clone();
landingLocation.setAltitude(getCurrentLocation().getAltitude());
return locationOverBase;
LinkedList<PositionVector> returnRoute = new LinkedList<PositionVector>();
returnRoute.add(landingLocation);
returnRoute.add(getBaseHoverLocation());
return returnRoute;
}
@Override
......@@ -294,7 +305,8 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
return;
this.setState(UAVstate.RETURN);
movement.setTargetVelocity(velocity);
movement.setTargetLocation(getBaseReturnLocation(), cb);
movement.setTargetLocationRoute(getBaseReturnRoute(), cb);
}
@Override
......@@ -329,7 +341,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
disconnectFromBase();
movement.setTargetVelocity(movement.getVerticalAscentMaxVelocity());
movement.setTargetLocation(getBaseReturnLocation(), cb);
movement.setTargetLocation(getBaseHoverLocation(), cb);
return true;
}
......@@ -453,7 +465,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public PositionVector getBaseLocation() {
return getBaseReturnLocation();
return baseLocation.clone();
}
......
src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java
View file @ 53248fdb
......@@ -54,7 +54,7 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
public class StateMulticopterMovement implements UAVMovementModel {
public enum MobilityState { GROUND, HOVER, ASCEND, DESCEND, TARGET};
private enum IntraState { STOP, ACCELERATION, CONSTANT, DECELERATION, FORWARD, TURN, CURVE};
private enum IntraState { STOP, ACCELERATION, CONSTANT, DECELERATION, TURN, CURVE, ADJUST};
private MobilityState state = MobilityState.GROUND;
private IntraState intrastate = IntraState.STOP;
......@@ -219,7 +219,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
///////////////////
private long approachTarget(long time) {
if(intrastate == IntraState.STOP) {
if(intrastate == IntraState.STOP || intrastate == IntraState.ADJUST) {
// not directly in front of the UAV... turn or curve
if(getHorizontalAngleToTarget() != 0) {
......@@ -285,8 +285,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
double a = getHorizontalMaxAcceleration();
// rough approximation for velocity induced drag when accelerating (half of maximum)
double a_dec = a + (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
// rough approximation for velocity induced drag when accelerating (half of maximum)
a = a - (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
double v0 = velocityVector.getLength();
......@@ -295,20 +294,26 @@ public class StateMulticopterMovement implements UAVMovementModel {
double accTime = (vMax - v0) / a;
double accDist = 0.5 * a * accTime * accTime + v0 * accTime;
double decTime = vMax / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + vMax * decTime;
if(route.size() == 1) {
double a_dec = a + (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
double decTime = vMax / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + vMax * decTime;
// need to limit acceleration time / maximum velocity
if(accDist + decDist > distanceToTarget) {
double A = 0.5 * a + (a * a)/(2 * a_dec);
double B = v0 + (v0 * a) / a_dec;
double C = (v0 * v0) / a_dec - distanceToTarget;
// need to limit acceleration time / maximum velocity
if(accDist + decDist > distanceToTarget) {
double A = 0.5 * a + (a * a)/(2 * a_dec);
double B = v0 + (v0 * a) / a_dec;
double C = (v0 * v0) / a_dec - distanceToTarget;
double t1 = (-B + Math.sqrt(B * B - 4 * A * C))/(2*A);
// re-calculate
accTime = t1;
double t1 = (-B + Math.sqrt(B * B - 4 * A * C))/(2*A);
// re-calculate
accTime = t1;
}
}
else {
assert accDist <= distanceToTarget;
}
long accelerationPhaseTime = (long) (accTime * Time.SECOND);
if(accelerationPhaseTime > time) {
......@@ -345,24 +350,43 @@ public class StateMulticopterMovement implements UAVMovementModel {
else if(intrastate == IntraState.CONSTANT)
{
assert velocityVector.getZ() == 0;
assert getHorizontalAngleToTarget() == 0;
if(getHorizontalAngleToTarget() != 0) {
System.out.println(Math.toDegrees(getHorizontalAngleToTarget()));
System.out.println();
}
double v = flightVelocity();
if(v != velocityVector.getLength()) {
if( Math.abs(v - velocityVector.getLength()) > Math.pow(10, -3)) {
throw new AssertionError("Not a rounding error!");
}
else {
System.out.println("Small deviation in velocity: " + velocityVector.getLength());
System.out.println();
}
}
assert v == velocityVector.getLength();
double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
// rough approximation for velocity induced drag when accelerating (half of maximum)
double a_dec = getHorizontalMaxAcceleration() + (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
// calculate brake time
double decTime = v / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + v * decTime;
double constDist = distanceToTarget - decDist;
double constDist = distanceToTarget;
double constTime = constDist / v;
// if last given point, need to break!
if(route.size() == 1) {
// rough approximation for velocity induced drag when accelerating (half of maximum)
double a_dec = getHorizontalMaxAcceleration() + (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
// calculate brake time
double decTime = v / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + v * decTime;
// substract the deceleration distance for breaking
constDist -= decDist;
constTime = constDist / v;
}
long constantPhaseTime = (long) (constTime * Time.SECOND);
if(constantPhaseTime > time) {
......@@ -371,11 +395,14 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
else {
System.out.println("CONSTANT for " + Time.getFormattedTime(constantPhaseTime));
intrastate = IntraState.DECELERATION;
// TODO
if(route.size() > 1) {
if(route.size() > 1 && (route.get(0).getAltitude() == route.get(1).getAltitude())) {
System.out.println("maybe should not decelerate...");
intrastate = IntraState.ADJUST;
}
else {
intrastate = IntraState.DECELERATION;
}
}
......@@ -389,6 +416,13 @@ public class StateMulticopterMovement implements UAVMovementModel {
velocityVector = directionVector.scale(v);
accelerationVector = directionVector.scale(0);
if(intrastate == IntraState.ADJUST) {
if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < Math.pow(10, -3)) {
positionVector = currentTargetPosition.clone();
locationReached(currentTargetPosition);
}
}
// thrust includes energy calculation;
motor.requestThrust(estimateRequiredThrust(v), constantPhaseTime);
......@@ -448,6 +482,177 @@ public class StateMulticopterMovement implements UAVMovementModel {
// remaining step time
time = time - decelerationtPhaseTime;
}
else if(intrastate == IntraState.CURVE) {
double v = flightVelocity();
if(velocityVector.getLength() != v) {
System.out.println("curve velocity is: " + velocityVector.getLength());
}
double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
PositionVector directionToTarget = currentTargetPosition.minus(positionVector);
double curveVelocity = Math.min(10, v);
double curveThrust = getHorizontalComponentMaxThrust() - estimateRequiredThrust(curveVelocity);
double turnRadius = ( mass * Math.pow(curveVelocity, 2)) / curveThrust;
// TODO
/*
* Tangente bestimmen, fliegen bis Tangentenpunkt, geradeaus?
*
*/
// calculate turn circle
double angle = getHorizontalAngleToTarget();
PositionVector circleCenter = null;
if(angle == 0) {
System.err.println("no");
}
else if(angle > 0) {
// left
System.out.println("left");
PositionVector directionToCircleCenter = new PositionVector(directionVector.getY(), -directionVector.getX());
directionToCircleCenter.normalize();
directionToCircleCenter.scale(turnRadius);
circleCenter = positionVector.plus(directionToCircleCenter);
System.out.println(circleCenter);
}
else if(angle < 0) {
//right
System.out.println("right");
PositionVector directionToCircleCenter = new PositionVector(-directionVector.getY(), directionVector.getX());
directionToCircleCenter.normalize();
directionToCircleCenter.multiplyScalar(turnRadius);
circleCenter = positionVector.plus(directionToCircleCenter);
System.out.println(circleCenter);
}
double circleCenterX = circleCenter.getX();
double circleCenterY = circleCenter.getY();
// https://stackoverflow.com/a/49987361
double targetX = currentTargetPosition.getX();
double targetY = currentTargetPosition.getY();
double b = Math.sqrt(Math.pow(targetX - circleCenterX, 2) + Math.pow(targetY - circleCenterY, 2));
double th = Math.acos(turnRadius / b); // angle theta
double d = Math.atan2(targetY - circleCenterY, targetX - circleCenterX); // direction angle of point P from C
double d1 = d + th; // direction angle of point T1 from C
double d2 = d - th; // direction angle of point T2 from C
double T1x = circleCenterX + turnRadius * Math.cos(d1);
double T1y = circleCenterY + turnRadius * Math.sin(d1);
double T2x = circleCenterX + turnRadius * Math.cos(d2);
double T2y = circleCenterY + turnRadius * Math.sin(d2);
PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ());
PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ());
System.out.println(T1);
System.out.println(T2);
System.out.println();
// now define which point to use...
double bearingT1 = positionVector.bearingTo(T1);
double bearingT2 = positionVector.bearingTo(T2);
System.out.println(Math.toDegrees(bearingT1));
System.out.println(Math.toDegrees(bearingT2));
PositionVector tangentPoint;
if(bearingT1 < bearingT2) {
tangentPoint = T1;
}
else {
tangentPoint = T2;
}
double circleAngle = 2 * Math.asin(positionVector.distanceTo(tangentPoint) / (2 * turnRadius));
System.out.println(Math.toDegrees(circleAngle));
double circleDistance = circleAngle * turnRadius;
System.out.println();
double constTime = circleDistance / curveVelocity;
long constantPhaseTime = (long) (constTime * Time.SECOND);
if(constantPhaseTime > time) {
System.out.println("CURVE phase exceed step time");
constantPhaseTime = time;
}
else {
System.out.println("CURVE for " + Time.getFormattedTime(constantPhaseTime));
}
constTime = (double) constantPhaseTime / Time.SECOND;
circleDistance = constTime * curveVelocity;
circleAngle = circleDistance / turnRadius;
System.out.println(Math.toDegrees(circleAngle));
double positionAngleX = Math.acos(( positionVector.getX() - circleCenterX) / turnRadius);
double positionAngleY = Math.asin(( positionVector.getY() - circleCenterY) / turnRadius);
double positionAngle = circleCenter.bearingTo(positionVector);
System.out.println(Math.toDegrees(positionAngle));
System.out.println(Math.toDegrees(positionAngle + circleAngle));
System.out.println();
double circleLocationX = circleCenterX + turnRadius * Math.cos(positionAngle + circleAngle);
double circleLocationY = circleCenterY + turnRadius * Math.sin(positionAngle + circleAngle);
currentAngleOfAttack = estimatePitchAngleForVelocity(curveVelocity);
// x' = x cos θ − y sin θ
// y' = x sin θ + y cos θ
double xn = directionVector.getX() * Math.cos(circleAngle) - directionVector.getY() * Math.sin(circleAngle);
double yn = directionVector.getX() * Math.sin(circleAngle) + directionVector.getY() * Math.cos(circleAngle);
directionVector = new PositionVector(xn, yn);
directionVector.normalize();
positionVector = new PositionVector(circleLocationX, circleLocationY, positionVector.getZ());
velocityVector = directionVector.scale(curveVelocity);
System.out.println(velocityVector.getLength());
PositionVector directionToCircleCenter = new PositionVector(-directionVector.getY(), directionVector.getX());
directionToCircleCenter.normalize();
accelerationVector = directionToCircleCenter.scale(curveThrust);
if(Math.abs(positionVector.distanceTo(tangentPoint)) < Math.pow(10, -3)) {
System.out.println(positionVector);
positionVector = tangentPoint.clone();
System.out.println(positionVector);
System.out.println();
System.out.println(directionVector);
directionVector = currentTargetPosition.minus(positionVector);
directionVector.normalize();
System.out.println(directionVector);
System.out.println();
intrastate = IntraState.ADJUST;
}
// thrust includes energy calculation; accelerate against flight direction
motor.requestThrust(getHorizontalComponentMaxThrust(), constantPhaseTime);
// remaining step time
time = time - constantPhaseTime;
}
System.out.println("Remaining steptime: " + time);
......@@ -631,6 +836,23 @@ public class StateMulticopterMovement implements UAVMovementModel {
double num = (directionToTarget.getX() * directionVector.getX() + directionToTarget.getY() * directionVector.getY());
double den = (Math.sqrt(Math.pow(directionToTarget.getX(), 2) + Math.pow(directionToTarget.getY(), 2)) * (Math.sqrt(Math.pow(directionVector.getX(), 2) + Math.pow(directionVector.getY(), 2))) );
double cos = num / den;
if(cos > 1) {
if(cos > 1 + Math.pow(10, -3)) {
throw new UnsupportedOperationException("cannot be bigger than 1!");
}
else {
cos = 1;
}
}
else if(cos < -1) {
if(cos < -1 - Math.pow(10, -3)) {
throw new UnsupportedOperationException("cannot be bigger than 1!");
}
else {
cos = -1;
}
}
/*
double dot = directionVector.getX() * - directionToTarget.getY() + directionVector.getY() * directionToTarget.getX();
......@@ -643,7 +865,8 @@ public class StateMulticopterMovement implements UAVMovementModel {
System.out.println("b parallel/antiparallel to a");
*/
double angle = - Math.acos(cos);
double angle = Math.acos(cos);
angle = -angle;
if(Math.abs(angle) < Math.pow(10, -3)) {
return 0;
......@@ -769,7 +992,12 @@ public class StateMulticopterMovement implements UAVMovementModel {
// remove current target position when reached
route.removeFirst();
currentTargetPosition = null;
if(!route.isEmpty()) {
currentTargetPosition = route.getFirst();
}
else {
currentTargetPosition = null;
}
if(locationCallbacks.containsKey(position)) {
locationCallbacks.get(position).reachedLocation();
......
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