Skip to content

GitLab

Commit f709146c authored by Julian Zobel's avatar Julian Zobel 🦄
Browse files

Acc/const/dec for straight flight

parent 8b4240c6
Hide whitespace changes
Inline Side-by-side
src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java
View file @ f709146c
......@@ -62,9 +62,8 @@ public class StateMulticopterMovement implements UAVMovementModel {
private UAVTopologyComponent topologyComponent;
private double currentAngleOfAttack;
private double velocity;
private double targetVelocity;
private double targetVelocity = -1;
private PositionVector currentTargetPosition = null;
......@@ -187,13 +186,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
update();
}
//double distanceToTargetPosition = positionVector.distanceTo(target);
if(timeBetweenMovementOperations != 0) {
System.out.println("Time Remaining....");
move(timeBetweenMovementOperations);
......@@ -201,107 +194,10 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
/*
// If target point is reached within a 1 meter margin, we remove that point from the list
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < velocity)
{
target = route.removeFirst();
if(route.isEmpty()) {
// go to hover mode
topologyComponent.updateCurrentLocation(target.clone());
velocity = 0;
motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations);
PositionVector direction = topologyComponent.getCurrentDirection().clone();
direction.setEntry(2, 0);
topologyComponent.updateCurrentDirection(direction);
locationReached(topologyComponent.getCurrentLocation());
return;
}
else {
// get to speed
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations);
velocity = targetVelocity;
}
else {
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations);
velocity = getHorizontalMaxVelocity();
}
long timeUntilReachedLocation = (long) (distanceToTargetPosition / velocity) * Time.SECOND;
target = route.getFirst();
PositionVector directionToTarget = new PositionVector(target);
directionToTarget.subtract(position);
double timefactor = timeUntilReachedLocation / Time.SECOND;
directionToTarget.normalize();
topologyComponent.updateCurrentDirection(directionToTarget.clone());
directionToTarget.multiplyScalar(velocity * timefactor);
PositionVector newPosition = new PositionVector(position);
newPosition.add(directionToTarget);
topologyComponent.updateCurrentLocation(newPosition);
if(timeUntilReachedLocation < timeBetweenMovementOperations) {
this.move(timeBetweenMovementOperations - timeUntilReachedLocation);
}
}
}
else {
*/
/*
double timefactor = timeBetweenMovementOperations / Time.SECOND;
// get to speed
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
velocity = targetVelocity;
}
else {
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
velocity = getHorizontalMaxVelocity();
}
PositionVector directionToTarget = new PositionVector(target);
directionToTarget.subtract(position);
directionToTarget.normalize();
if(directionToTarget.getX() != 0 || directionToTarget.getY() != 0) {
topologyComponent.updateCurrentDirection(directionToTarget.clone());
}
directionToTarget.multiplyScalar(velocity * timefactor);
PositionVector newPosition = new PositionVector(position);
newPosition.add(directionToTarget);
topologyComponent.updateCurrentLocation(newPosition);
*/
}
else if(motor.isOn()) {
if(velocity != 0) {
if(velocityVector.getLength() != 0) {
throw new UnsupportedOperationException("[StateMulticopterMovement] no route but speed not 0?");
}
......@@ -325,46 +221,33 @@ public class StateMulticopterMovement implements UAVMovementModel {
if(intrastate == IntraState.STOP) {
if(velocityVector.getLength() == 0) {
System.out.println("turn before acc");
intrastate = IntraState.TURN;
// not directly in front of the UAV... turn or curve
if(getHorizontalAngleToTarget() != 0) {
if(velocityVector.getLength() == 0) {
System.out.println("turn before acc");
intrastate = IntraState.TURN;
}
else {
System.out.println(velocityVector.getLength());
// TODO
intrastate = IntraState.CURVE;
}
}
else {
System.out.println(velocityVector.getLength());
// TODO
System.out.println();
intrastate = IntraState.ACCELERATION;
}
}
else if(intrastate == IntraState.TURN) {
double rot = getHorizontalAngleToTarget();
double turnTime = Math.abs(rot) / maximumTurnAngle;
long turnPhaseTime = (long) (turnTime * Time.SECOND);
System.out.println(directionVector);
PositionVector directionToTarget = currentTargetPosition.minus(positionVector);
directionToTarget.normalize();
System.out.println(directionToTarget);
System.out.println();
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;
double dot = directionVector.getX() * -directionToTarget.getY() + directionVector.getY() * directionToTarget.getX();
if(dot > 0)
System.out.println("b on the left of a");
else if(dot < 0)
System.out.println("b on the right of a");
else
System.out.println("b parallel/antiparallel to a");
double rot= Math.acos(cos);
System.out.println(Math.toDegrees(rot));
double turnTime = rot / maximumTurnAngle;
long turnPhaseTime = (long) (turnTime * Time.SECOND);
if(turnPhaseTime > time) {
System.out.println("Turning phase exceed step time");
......@@ -376,19 +259,15 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
turnTime = (double) turnPhaseTime / Time.SECOND;
rot = rot * ( turnTime / (rot / maximumTurnAngle) );
rot = rot * ( turnTime / (Math.abs(rot) / maximumTurnAngle) );
// x' = x cos θ − y sin θ
// y' = x sin θ + y cos θ
double xn = directionVector.getX() * Math.cos(-rot) - directionVector.getY() * Math.sin(-rot);
double yn = directionVector.getX() * Math.sin(-rot) + directionVector.getY() * Math.cos(-rot);
System.out.println(new PositionVector(xn, yn));
System.out.println();
double xn = directionVector.getX() * Math.cos(rot) - directionVector.getY() * Math.sin(rot);
double yn = directionVector.getX() * Math.sin(rot) + directionVector.getY() * Math.cos(rot);
directionVector = new PositionVector(xn, yn);
directionVector.normalize();
// thrust includes energy calculation;
motor.requestThrust(hoverThrustRequired(), turnPhaseTime);
......@@ -397,6 +276,179 @@ public class StateMulticopterMovement implements UAVMovementModel {
time = time - turnPhaseTime;
System.out.println();
}
else if(intrastate == IntraState.ACCELERATION)
{
// acceleration
assert velocityVector.getZ() == 0;
assert getHorizontalAngleToTarget() == 0;
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;
a = a - (0.5 * bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)) * (getHorizontalMaxVelocity() * getHorizontalMaxVelocity()) / 4) / mass;
double v0 = velocityVector.getLength();
double vMax = flightVelocity();
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;
// 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;
}
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 * accTime * accTime + v0 * accTime;
double vint = Math.min(vMax, (a * accTime + v0));
if(Math.abs(vint - vMax) < Math.pow(10, -3)) {
vint = vMax;
}
currentAngleOfAttack = estimatePitchAngleForVelocity(vint);
directionVector.normalize(); // should be valid anyways?
positionVector = positionVector.plus(directionVector.scale(accDist));
velocityVector = directionVector.scale(vint);
accelerationVector = directionVector.scale(a);
// thrust includes energy calculation; accelerate towards target
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), accelerationPhaseTime);
// remaining step time
time = time - accelerationPhaseTime;
}
else if(intrastate == IntraState.CONSTANT)
{
assert velocityVector.getZ() == 0;
assert getHorizontalAngleToTarget() == 0;
double v = flightVelocity();
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 constTime = constDist / v;
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;
// TODO
if(route.size() > 1) {
System.out.println("maybe should not decelerate...");
}
}
constTime = (double) constantPhaseTime / Time.SECOND;
constDist = constTime * v;
//currentAngleOfAttack = estimatePitchAngleForVelocity(v);
directionVector.normalize(); // should be valid anyways?
positionVector = positionVector.plus(directionVector.scale(constDist));
velocityVector = directionVector.scale(v);
accelerationVector = directionVector.scale(0);
// thrust includes energy calculation;
motor.requestThrust(estimateRequiredThrust(v), constantPhaseTime);
// remaining step time
time = time - constantPhaseTime;
}
else if(intrastate == IntraState.DECELERATION)
{
assert velocityVector.getZ() == 0;
assert getHorizontalAngleToTarget() == 0;
double v = velocityVector.getLength();
// 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;
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 + v * decTime;
double vint = (v - decTime * a_dec);
if(vint < Math.pow(10, -3)) {
vint = 0;
}
directionVector.normalize(); // should be valid anyways?
positionVector = positionVector.plus(directionVector.scale(decDist));
velocityVector = directionVector.scale(vint);
accelerationVector = directionVector.scale(-a_dec);
// 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;
positionVector = currentTargetPosition.clone();
velocityVector = directionVector.scale(0);
accelerationVector = directionVector.scale(0);
this.state = MobilityState.HOVER;
}
// remaining step time
time = time - decelerationtPhaseTime;
}
System.out.println("Remaining steptime: " + time);
if(time > 0 && intrastate != IntraState.STOP) {
......@@ -434,7 +486,6 @@ public class StateMulticopterMovement implements UAVMovementModel {
{
// acceleration
double v0 = velocityVector.getZ();
double a0 = accelerationVector.getZ();
double a = verticalAscentMaxAcceleration();
double vMax = getVerticalAscentMaxVelocity();
......@@ -566,7 +617,45 @@ public class StateMulticopterMovement implements UAVMovementModel {
return steptime;
}
///////////
///////////
/**
*
* @return
*/
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 cos = num / den;
/*
double dot = directionVector.getX() * - directionToTarget.getY() + directionVector.getY() * directionToTarget.getX();
if(dot > 0)
System.out.println("b on the left of a");
else if(dot < 0)
System.out.println("b on the right of a");
else
System.out.println("b parallel/antiparallel to a");
*/
double angle = - Math.acos(cos);
if(Math.abs(angle) < Math.pow(10, -3)) {
return 0;
}
else {
return angle;
}
}
//////////
//////////
/*
......@@ -593,25 +682,23 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
@Override
public double getHorizontalMaxVelocity() {
double horizontalThrust = getHorizontalComponentMaxThrust();
double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)));
public double getHorizontalMaxVelocity() {
double horizontalThrust = getHorizontalComponentMaxThrust();
double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)));
return maxVelocity;
}
protected double getHorizontalComponentMaxThrust() {
protected double getHorizontalComponentMaxThrust() {
// hoverthrust / cos => amount of thrust in horizonal direction with °angle
double stableAltitudeMaximumTotalThrust = getHorizontalMaxVelocityRequiredTotalThrust();
// fraction of total thrust in horizonal (forward) direction with °angle
double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed);
double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed);
return maximumHorizontalThrustStableAltitude;
}
protected double getHorizontalMaxAcceleration() {
return getHorizontalComponentMaxThrust() / mass;
}
protected double getHorizontalMaxVelocityRequiredTotalThrust() {
return hoverThrustRequired() / Math.cos(maximumPitchAngleAllowed);
......@@ -636,7 +723,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
* F_drag [N] = 0.5 * p * C_drag * A * v^2
*/
protected double currentDrag() {
return 0.5 * bodyDrag(currentAngleOfAttack, topologyComponent.getCurrentDirection()) * velocity * velocity;
return 0.5 * bodyDrag(currentAngleOfAttack, directionVector) * velocityVector.getLength() * velocityVector.getLength();
}
/**
......@@ -667,7 +754,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
@Override
public double getCurrentVelocity() {
return velocity;
return velocityVector.getLength() ;
}
/**
......@@ -678,8 +765,10 @@ public class StateMulticopterMovement implements UAVMovementModel {
private void locationReached(PositionVector position) {
assert position == currentTargetPosition;
assert position == route.getFirst();
// remove current target position when reached
// remove current target position when reached
route.removeFirst();
currentTargetPosition = null;
if(locationCallbacks.containsKey(position)) {
......@@ -833,6 +922,16 @@ public class StateMulticopterMovement implements UAVMovementModel {
return maximumPitchAngleAllowed;
}
private double flightVelocity() {
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
return targetVelocity;
}
else {
targetVelocity = -1;
return getHorizontalMaxVelocity();
}
}
/**
* Factory for this movement model
*
......
src/de/tud/kom/p2psim/impl/topology/util/PositionVector.java
View file @ f709146c
......@@ -365,6 +365,22 @@ public class PositionVector implements Location {
}
return result;
}
/**
* Multiply this vector with a scalar value. Current vector is
* not changed. If you want the current vector instance to change, you
* should use add instead.
*
* @param multi
* @return the vector
*/
public PositionVector scale(double multi) {
PositionVector result = new PositionVector(getDimensions());
for (int i = 0; i < getDimensions(); i++) {
result.setEntry(i, multi * getEntry(i));
}
return result;
}
@Override
public int getTransmissionSize() {
......
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