Skip to content

GitLab

Commit 8e49b0e2 authored by Julian Zobel's avatar Julian Zobel 🦄
Browse files

Some bugs in left curve

parent 53248fdb
Hide whitespace changes
Inline Side-by-side
src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java
View file @ 8e49b0e2
......@@ -142,9 +142,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
else {
// TODO turn or move
this.state = MobilityState.TARGET;
}
}
}
else {
......@@ -152,6 +150,10 @@ public class StateMulticopterMovement implements UAVMovementModel {
case ASCEND:
timeBetweenMovementOperations = ascend(timeBetweenMovementOperations);
break;
case DESCEND:
// TODO
timeBetweenMovementOperations = descend(timeBetweenMovementOperations);
break;
case HOVER:
if(positionVector.distanceTo(currentTargetPosition) == 0) {
System.out.println("location reached");
......@@ -371,10 +373,11 @@ public class StateMulticopterMovement implements UAVMovementModel {
double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
double constDist = distanceToTarget;
double constTime = constDist / v;
boolean decelerationRequired = decelerationAtNextTargetRequired();
// if last given point, need to break!
if(route.size() == 1) {
if(decelerationRequired) {
// 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;
......@@ -397,7 +400,8 @@ public class StateMulticopterMovement implements UAVMovementModel {
System.out.println("CONSTANT for " + Time.getFormattedTime(constantPhaseTime));
// TODO
if(route.size() > 1 && (route.get(0).getAltitude() == route.get(1).getAltitude())) {
if(!decelerationRequired) {
//if(route.size() > 1 && (route.get(0).getAltitude() == route.get(1).getAltitude())) {
System.out.println("maybe should not decelerate...");
intrastate = IntraState.ADJUST;
}
......@@ -417,7 +421,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
accelerationVector = directionVector.scale(0);
if(intrastate == IntraState.ADJUST) {
if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < Math.pow(10, -3)) {
if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < Math.pow(10, -2)) {
positionVector = currentTargetPosition.clone();
locationReached(currentTargetPosition);
}
......@@ -432,7 +436,10 @@ public class StateMulticopterMovement implements UAVMovementModel {
else if(intrastate == IntraState.DECELERATION)
{
assert velocityVector.getZ() == 0;
assert getHorizontalAngleToTarget() == 0;
if(getHorizontalAngleToTarget() != 0) {
System.out.println(getHorizontalAngleToTarget());
}
double v = velocityVector.getLength();
......@@ -451,7 +458,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
else {
System.out.println("deceleration for " + Time.getFormattedTime(decelerationtPhaseTime));
intrastate = IntraState.STOP;
}
decTime = (double) decelerationtPhaseTime / Time.SECOND;
......@@ -470,8 +477,8 @@ public class StateMulticopterMovement implements UAVMovementModel {
// thrust includes energy calculation; accelerate against flight direction
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), decelerationtPhaseTime);
if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < Math.pow(10, -3)) {
assert intrastate == IntraState.STOP;
if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < Math.pow(10, -2)) {
intrastate = IntraState.STOP;
positionVector = currentTargetPosition.clone();
velocityVector = directionVector.scale(0);
accelerationVector = directionVector.scale(0);
......@@ -489,6 +496,11 @@ public class StateMulticopterMovement implements UAVMovementModel {
System.out.println("curve velocity is: " + velocityVector.getLength());
}
if(currentTargetPosition.getX() == 1000) {
System.out.println();
}
double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
PositionVector directionToTarget = currentTargetPosition.minus(positionVector);
......@@ -508,20 +520,21 @@ public class StateMulticopterMovement implements UAVMovementModel {
double angle = getHorizontalAngleToTarget();
PositionVector circleCenter = null;
if(angle == 0) {
System.err.println("no");
}
else if(angle > 0) {
else if(angle < 0) {
// left
System.out.println("left");
PositionVector directionToCircleCenter = new PositionVector(directionVector.getY(), -directionVector.getX());
directionToCircleCenter.normalize();
directionToCircleCenter.scale(turnRadius);
directionToCircleCenter.multiplyScalar(turnRadius);
circleCenter = positionVector.plus(directionToCircleCenter);
System.out.println(circleCenter);
}
else if(angle < 0) {
else if(angle > 0) {
//right
System.out.println("right");
......@@ -567,7 +580,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
PositionVector tangentPoint;
if(bearingT1 < bearingT2) {
if(Math.abs(bearingT1) < Math.abs(bearingT2)) {
tangentPoint = T1;
}
else {
......@@ -663,7 +676,42 @@ public class StateMulticopterMovement implements UAVMovementModel {
return time;
}
private boolean decelerationAtNextTargetRequired() {
if(route == null || route.size() == 0) {
throw new UnsupportedOperationException("Cannot be possible");
}
if(route.size() == 1) {
return true;
}
switch(state) {
case HOVER:
case GROUND:
// not really interesting at this point!
return true;
//
case ASCEND:
if(route.get(0).getAltitude() < route.get(1).getAltitude()) {
return false;
}
break;
//
case DESCEND:
if(route.get(0).getAltitude() > route.get(1).getAltitude()) {
return false;
}
break;
case TARGET:
if(route.get(0).getAltitude() == route.get(1).getAltitude()) {
return false;
}
break;
}
return true;
}
private void hover(long time) {
this.state = MobilityState.HOVER;
......@@ -675,11 +723,167 @@ public class StateMulticopterMovement implements UAVMovementModel {
motor.requestThrust(0, time);
}
private long descend(long time) {
assert time > 0;
assert positionVector.getAltitude() > currentTargetPosition.getAltitude();
assert this.state == MobilityState.DESCEND;
double verticalDistance = positionVector.getAltitude() - currentTargetPosition.getAltitude();
if(intrastate == IntraState.STOP)
{
intrastate = IntraState.ACCELERATION;
}
else if(intrastate == IntraState.ACCELERATION)
{
// acceleration
double v0 = velocityVector.getZ();
double a_acc = GRAVITY;
double a_dec = verticalAscentMaxAcceleration();
double vMax = maximumDescentVelocityAllowed;
double accTime = (vMax - v0) / a_acc;
double accDist = 0.5 * a_acc * accTime * accTime + v0 * accTime;
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 > verticalDistance) {
double A = 0.5 * a_acc + (a_acc * a_acc)/(2 * a_dec);
double B = v0 + (v0 * a_acc) / a_dec;
double C = (v0 * v0) / a_dec - verticalDistance;
double t1 = (-B + Math.sqrt(B * B - 4 * A * C))/(2*A);
// re-calculate
accTime = t1;
}
long accelerationPhaseTime = (long) (accTime * Time.SECOND);
if(accelerationPhaseTime > time) {
System.out.println("acceleration phase exceed step time");
accelerationPhaseTime = time;
}
else {
System.out.println("Acceleration for " + Time.getFormattedTime(accelerationPhaseTime));
intrastate = IntraState.CONSTANT;
}
accTime = (double) accelerationPhaseTime / Time.SECOND;
accDist = 0.5 * a_acc * accTime * accTime + v0 * accTime;
double vint = (a_acc * accTime + v0);
positionVector.setZ(positionVector.getAltitude() - accDist);
velocityVector.setZ(-vint);
accelerationVector.setZ(-a_acc);
// thrust includes energy calculation;
motor.requestThrust(motor.getMinThrust(), accelerationPhaseTime);
// remaining step time
time = time - accelerationPhaseTime;
}
else if(intrastate == IntraState.CONSTANT)
{
double v0 = Math.abs(velocityVector.getZ());
if(v0 > maximumDescentVelocityAllowed) {
System.out.println("err");
}
// assert v0 < maximumDescentVelocityAlloweds
double a_dec = verticalAscentMaxAcceleration();
// calculate brake time
double decTime = v0 / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + v0 * decTime;
double constDist = verticalDistance - decDist;
double constTime = constDist / v0;
long constantPhaseTime = (long) (constTime * Time.SECOND);
if(constantPhaseTime > time) {
System.out.println("constant phase exceed step time");
constantPhaseTime = time;
}
else {
System.out.println("CONSTANT for " + Time.getFormattedTime(constantPhaseTime));
intrastate = IntraState.DECELERATION;
}
constTime = (double) constantPhaseTime / Time.SECOND;
constDist = constTime * v0;
positionVector.setZ(positionVector.getAltitude() - constDist);
// thrust includes energy calculation;
motor.requestThrust(verticalDescentThrust(), constantPhaseTime);
// remaining step time
time = time - constantPhaseTime;
}
else if(intrastate == IntraState.DECELERATION)
{
double v0 = Math.abs(velocityVector.getZ());
double a_dec = verticalAscentMaxAcceleration();
// calculate brake time
double decTime = v0 / a_dec;
double decDist = -0.5 * a_dec * decTime * decTime + v0 * decTime;
long decelerationtPhaseTime = (long) (decTime * Time.SECOND);
if(decelerationtPhaseTime > time) {
System.out.println("deceleration phase exceed step time");
decelerationtPhaseTime = time;
}
else {
System.out.println("deceleration for " + Time.getFormattedTime(decelerationtPhaseTime));
intrastate = IntraState.STOP;
}
decTime = (double) decelerationtPhaseTime / Time.SECOND;
decDist = -0.5 * a_dec * decTime * decTime + v0 * decTime;
double vint = (v0 - decTime * a_dec);
positionVector.setZ(positionVector.getAltitude() - decDist);
velocityVector.setZ(-vint);
accelerationVector.setZ(a_dec);
// thrust includes energy calculation;
motor.requestThrust(motor.getMaxThrust(), decelerationtPhaseTime);
// reached the position
if(Math.abs(currentTargetPosition.getAltitude() - positionVector.getAltitude()) < Math.pow(10, -3)) {
assert intrastate == IntraState.STOP;
positionVector.setZ(currentTargetPosition.getAltitude());
velocityVector.setZ(0);
accelerationVector.setZ(0);
this.state = MobilityState.HOVER;
}
// remaining step time
time = time - decelerationtPhaseTime;
}
System.out.println("Remaining steptime: " + time);
if(time > 0 && intrastate != IntraState.STOP) {
time = descend(time);
}
return time;
}
private long ascend(long steptime) {
assert steptime > 0;
assert positionVector.getAltitude() < currentTargetPosition.getAltitude();
assert this.state == MobilityState.ASCEND;
assert this.state == MobilityState.ASCEND;
double verticalDistance = currentTargetPosition.getAltitude() - positionVector.getAltitude();
......@@ -827,16 +1031,21 @@ public class StateMulticopterMovement implements UAVMovementModel {
/**
*
* @return
* @return >0 -> right; <0 -> left
*/
private double getHorizontalAngleToTarget() {
PositionVector directionToTarget = currentTargetPosition.minus(positionVector);
directionToTarget.normalize();
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 den = (directionToTarget.getLength() * directionVector.getLength());
double cos = num / den;
double dot1 = directionVector.getX() * directionToTarget.getX() + directionVector.getY() * directionToTarget.getY(); //dot product between [x1, y1] and [x2, y2]
double det1 = directionVector.getX() * directionToTarget.getY() - directionVector.getY() * directionToTarget.getX(); // determinant
double angle2 = Math.atan2(det1, dot1); // atan2(y, x) or atan2(sin, cos)
System.out.println(Math.toDegrees(angle2));
if(cos > 1) {
if(cos > 1 + Math.pow(10, -3)) {
throw new UnsupportedOperationException("cannot be bigger than 1!");
......@@ -867,12 +1076,14 @@ public class StateMulticopterMovement implements UAVMovementModel {
double angle = Math.acos(cos);
angle = -angle;
System.out.println(Math.toDegrees(angle));
if(Math.abs(angle) < Math.pow(10, -3)) {
if(Math.abs(angle2) < Math.pow(10, -3)) {
return 0;
}
else {
return angle;
return angle2;
}
}
......@@ -884,9 +1095,19 @@ public class StateMulticopterMovement implements UAVMovementModel {
/*
*
*/
protected double verticalDescentMaxThrust() {
// m * g - 0.5 * p * C * A * v^2
return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * maximumDescentVelocityAllowed * maximumDescentVelocityAllowed;
// Thrust required to hold the maximum allowed descent velocity
protected double verticalDescentThrust() {
return hoverThrustRequired() - verticalDescentDrag();
}
// Drag (upward lift) induced on the frame by moving with the maximum allowed descent velocity
// 0.5 * p * C * A * v^2
protected double verticalDescentDrag() {
return 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * maximumDescentVelocityAllowed * maximumDescentVelocityAllowed;
}
protected double verticalDescentMaxAcceleration() {
return verticalDescentDrag() / mass;
}
protected double verticalAscentMaxAcceleration() {
......
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