Skip to content

GitLab

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

Turning started

parent dedb49af
Hide whitespace changes
Inline Side-by-side
src/de/tud/kom/p2psim/impl/energy/components/StatelessActuatorComponent.java
View file @ 8b4240c6
......@@ -47,7 +47,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
private EnergyEventListener energyModel;
private long lastEnergyConsumationTime;
//private long lastEnergyConsumationTime;
private double volts;
private final double uJconversionFactor = 1000000;
......@@ -63,7 +63,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
this.volts = volt;
this.numberOfActuators = numberOfActuators;
this.state = componentState.OFF;
this.lastEnergyConsumationTime = Time.getCurrentTime();
//this.lastEnergyConsumationTime = Time.getCurrentTime();
thrust = 0;
amps = 0;
......@@ -89,7 +89,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
/**
* Set the new energy state and calculate the energy consumption from the last state
*/
private void setEnergyState() {
private void setEnergyState(long stateDuration) {
// set the new energy state
EnergyState newState;
......@@ -102,25 +102,26 @@ public class StatelessActuatorComponent implements ActuatorComponent {
}
// calculate energy consumption for the previous state
long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationTime;
//long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationTime;
double cons = calculateEnergyConsumation(energyState, timeSpentInState);
double cons = calculateEnergyConsumation(energyState, stateDuration);
energyModel.componentConsumedEnergy(this, cons);
// set new state
energyState = newState;
lastEnergyConsumationTime = Time.getCurrentTime();
//lastEnergyConsumationTime = Time.getCurrentTime();
}
/**
* Request a given amount of thrust to be provided from this component. If the amount is less than the minimum
* or more than the maximum, the minimum or maximum thrust values, respectively, are enforced.
*
* @param targetThrust
* @param targetThrust Requested Thrust
* @param thrustDuration Time the thrust is needed
* @return The amount of thrust this component now generates.
*/
public double requestThrust(double targetThrust) {
public double requestThrust(double targetThrust, long thrustDuration) {
if(targetThrust == 0 || targetThrust <= numberOfActuators * characteristics.getFirst().getThrust()) {
setLoad(characteristics.getFirst());
......@@ -132,6 +133,18 @@ public class StatelessActuatorComponent implements ActuatorComponent {
calculateAndSetThrustRelatedAmpereDraw(targetThrust);
}
this.setEnergyState(thrustDuration);
return this.thrust;
}
/**
* Request the current thrust for a certain time.
* @param thrustDuration
* @return
*/
public double requestThrust(long thrustDuration) {
this.setEnergyState(thrustDuration);
return this.thrust;
}
......@@ -220,13 +233,11 @@ public class StatelessActuatorComponent implements ActuatorComponent {
private void setLoad(double thrust, double amps) {
this.thrust = thrust;
this.amps = amps;
setEnergyState();
}
private void setLoad(MotorCharacteristic ch) {
this.thrust = ch.getThrust();
this.amps = ch.getCurrent();
setEnergyState();
this.amps = ch.getCurrent();
}
/**
......@@ -266,7 +277,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
this.thrust = 0;
this.amps = 0;
this.state = componentState.OFF;
setEnergyState();
setEnergyState(0);
return true;
}
......@@ -276,7 +287,7 @@ public class StatelessActuatorComponent implements ActuatorComponent {
if (isAvailable()) {
if(this.state != componentState.ON) {
this.state = componentState.ON;
requestThrust(0);
requestThrust(0, 0);
}
return true;
}
......
src/de/tud/kom/p2psim/impl/topology/movement/aerial/MulticopterMovement.java
View file @ 8b4240c6
......@@ -113,7 +113,7 @@ public class MulticopterMovement implements UAVMovementModel {
topologyComponent.updateCurrentLocation(target.clone());
velocity = 0;
motor.requestThrust(hoverThrustRequired());
motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations);
PositionVector direction = topologyComponent.getCurrentDirection().clone();
direction.setEntry(2, 0);
......@@ -126,11 +126,11 @@ public class MulticopterMovement implements UAVMovementModel {
// get to speed
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations);
velocity = targetVelocity;
}
else {
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations);
velocity = getHorizontalMaxVelocity();
}
......@@ -163,11 +163,11 @@ public class MulticopterMovement implements UAVMovementModel {
// get to speed
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations);
velocity = targetVelocity;
}
else {
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations);
velocity = getHorizontalMaxVelocity();
}
......@@ -198,10 +198,10 @@ public class MulticopterMovement implements UAVMovementModel {
PositionVector position = new PositionVector(topologyComponent.getRealPosition());
if(position.getAltitude() == 0) {
motor.requestThrust(0);
motor.requestThrust(0, timeBetweenMovementOperations);
}
else {
motor.requestThrust(hoverThrustRequired());
motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations);
}
topologyComponent.updateCurrentLocation(position);
......
src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java
View file @ 8b4240c6
......@@ -29,6 +29,8 @@ import javax.persistence.Column;
import javax.persistence.Entity;
import javax.persistence.Table;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
import org.jboss.logging.annotations.Pos;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
......@@ -51,8 +53,8 @@ import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocation
*/
public class StateMulticopterMovement implements UAVMovementModel {
public enum MobilityState { GROUND, HOVER, ASCEND, DESCEND, ACCELERATE, DECELERATE, FORWARD, TURN, CURVE };
private enum IntraState { STOP, ACCELERATION, CONSTANT, DECELERATION };
public enum MobilityState { GROUND, HOVER, ASCEND, DESCEND, TARGET};
private enum IntraState { STOP, ACCELERATION, CONSTANT, DECELERATION, FORWARD, TURN, CURVE};
private MobilityState state = MobilityState.GROUND;
private IntraState intrastate = IntraState.STOP;
......@@ -64,6 +66,8 @@ public class StateMulticopterMovement implements UAVMovementModel {
private double targetVelocity;
private PositionVector currentTargetPosition = null;
private LinkedList<PositionVector> route = new LinkedList<>();
private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>(); // TODO callback interface
......@@ -108,6 +112,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
private void update() {
System.out.println("Updaing location: " + positionVector);
topologyComponent.updateCurrentLocation(new PositionVector(positionVector));
topologyComponent.updateCurrentDirection(new PositionVector(directionVector));
}
......@@ -121,13 +126,82 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
if(motor.isOn() && !route.isEmpty()) {
PositionVector position = new PositionVector(topologyComponent.getRealPosition());
assert positionVector.distanceTo(topologyComponent.getRealPosition()) == 0;
if(currentTargetPosition == null) {
currentTargetPosition = route.getFirst();
// new target, check
if(positionVector.getAltitude() < currentTargetPosition.getAltitude()) {
System.out.println("up");
this.state = MobilityState.ASCEND;
}
else if(positionVector.getAltitude() > currentTargetPosition.getAltitude()) {
// TODO lower altitude
this.state = MobilityState.DESCEND;
}
else {
// TODO turn or move
this.state = MobilityState.TARGET;
}
}
else {
switch( state) {
case ASCEND:
timeBetweenMovementOperations = ascend(timeBetweenMovementOperations);
break;
case HOVER:
if(positionVector.distanceTo(currentTargetPosition) == 0) {
System.out.println("location reached");
this.locationReached(currentTargetPosition);
}
else {
// FIXME doubled
// new target, check
if(positionVector.getAltitude() < currentTargetPosition.getAltitude()) {
System.out.println("up");
this.state = MobilityState.ASCEND;
}
else if(positionVector.getAltitude() > currentTargetPosition.getAltitude()) {
// TODO lower altitude
this.state = MobilityState.DESCEND;
}
else {
// TODO turn or move
this.state = MobilityState.TARGET;
}
PositionVector target = route.getFirst();
//throw new UnsupportedOperationException("cannot be in hover when not reached the target location!");
}
break;
case TARGET:
timeBetweenMovementOperations = approachTarget(timeBetweenMovementOperations);
break;
default: System.out.println("current state " + state);
}
update();
}
//double distanceToTargetPosition = positionVector.distanceTo(target);
double distanceToTargetPosition = position.distanceTo(target);
if(timeBetweenMovementOperations != 0) {
System.out.println("Time Remaining....");
move(timeBetweenMovementOperations);
return;
}
/*
// If target point is reached within a 1 meter margin, we remove that point from the list
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < velocity)
{
......@@ -139,7 +213,7 @@ public class StateMulticopterMovement implements UAVMovementModel {
topologyComponent.updateCurrentLocation(target.clone());
velocity = 0;
motor.requestThrust(hoverThrustRequired());
motor.requestThrust(hoverThrustRequired(), timeBetweenMovementOperations);
PositionVector direction = topologyComponent.getCurrentDirection().clone();
direction.setEntry(2, 0);
......@@ -152,11 +226,11 @@ public class StateMulticopterMovement implements UAVMovementModel {
// get to speed
if(targetVelocity > 0 && targetVelocity < getHorizontalMaxVelocity()) {
motor.requestThrust(estimateRequiredThrust(targetVelocity));
motor.requestThrust(estimateRequiredThrust(targetVelocity), timeBetweenMovementOperations);
velocity = targetVelocity;
}
else {
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust());
motor.requestThrust(getHorizontalMaxVelocityRequiredTotalThrust(), timeBetweenMovementOperations);
velocity = getHorizontalMaxVelocity();
}
......@@ -185,11 +259,9 @@ public class StateMulticopterMovement implements UAVMovementModel {
}
else {
*/
if(position.getAltitude() < target.getAltitude()) {
ascend(timeBetweenMovementOperations);
update();
}
/*
......@@ -222,13 +294,15 @@ public class StateMulticopterMovement implements UAVMovementModel {
topologyComponent.updateCurrentLocation(newPosition);
*/
}
}
else if(motor.isOn()) {
if(velocity != 0) {
throw new UnsupportedOperationException("[MulticopterMovement] no route but speed not 0?");
throw new UnsupportedOperationException("[StateMulticopterMovement] no route but speed not 0?");
}
PositionVector position = new PositionVector(topologyComponent.getRealPosition());
......@@ -247,30 +321,113 @@ public class StateMulticopterMovement implements UAVMovementModel {
///////////////////
///////////////////
///////////////////
private long approachTarget(long time) {
if(intrastate == IntraState.STOP) {
if(velocityVector.getLength() == 0) {
System.out.println("turn before acc");
intrastate = IntraState.TURN;
}
else {
System.out.println(velocityVector.getLength());
// TODO
}
}
else if(intrastate == IntraState.TURN) {
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");
turnPhaseTime = time;
}
else {
System.out.println("Turning for " + Time.getFormattedTime(turnPhaseTime));
intrastate = IntraState.STOP;
}
turnTime = (double) turnPhaseTime / Time.SECOND;
rot = rot * ( turnTime / (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();
directionVector = new PositionVector(xn, yn);
// thrust includes energy calculation;
motor.requestThrust(hoverThrustRequired(), turnPhaseTime);
// remaining step time
time = time - turnPhaseTime;
System.out.println();
}
System.out.println("Remaining steptime: " + time);
if(time > 0 && intrastate != IntraState.STOP) {
time = approachTarget(time);
}
return time;
}
private void hover(long time) {
this.state = MobilityState.HOVER;
motor.requestThrust(hoverThrustRequired());
motor.requestThrust(hoverThrustRequired(), time);
}
private void ground(long time) {
this.state = MobilityState.GROUND;
motor.requestThrust(0);
motor.requestThrust(0, time);
}
private long ascend(long steptime) {
assert steptime > 0;
assert positionVector.getAltitude() < currentTargetPosition.getAltitude();
assert this.state == MobilityState.ASCEND;
this.state = MobilityState.ASCEND;
PositionVector target = route.getFirst();
//this.positionVector = topologyComponent.getCurrentLocation();
assert positionVector.getAltitude() < target.getAltitude();
double verticalDistance = target.getAltitude() - positionVector.getAltitude();
double verticalDistance = currentTargetPosition.getAltitude() - positionVector.getAltitude();
if(intrastate == IntraState.STOP)
{
{
intrastate = IntraState.ACCELERATION;
}
else if(intrastate == IntraState.ACCELERATION)
......@@ -318,6 +475,10 @@ public class StateMulticopterMovement implements UAVMovementModel {
velocityVector.setZ(vint);
accelerationVector.setZ(a);
// thrust includes energy calculation;
motor.requestThrust(motor.getMaxThrust(), accelerationPhaseTime);
// remaining step time
steptime = steptime - accelerationPhaseTime;
}
else if(intrastate == IntraState.CONSTANT)
......@@ -342,7 +503,16 @@ public class StateMulticopterMovement implements UAVMovementModel {
intrastate = IntraState.DECELERATION;
}
constTime = (double) constantPhaseTime / Time.SECOND;
constDist = constTime * v0;
positionVector.setZ(positionVector.getAltitude() + constDist);
// thrust includes energy calculation;
motor.requestThrust(constantPhaseTime);
// remaining step time
steptime = steptime - constantPhaseTime;
}
else if(intrastate == IntraState.DECELERATION)
{
......@@ -368,14 +538,23 @@ public class StateMulticopterMovement implements UAVMovementModel {
double vint = (v0 - decTime * GRAVITY);
positionVector.setZ(positionVector.getAltitude() + decDist);
if(Math.abs(target.getAltitude() - positionVector.getAltitude()) < Math.pow(10, -3)) {
positionVector.setZ(target.getAltitude());
}
velocityVector.setZ(vint);
accelerationVector.setZ(GRAVITY);
// thrust includes energy calculation;
motor.requestThrust(motor.getMinThrust(), decelerationtPhaseTime);
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
steptime = steptime - decelerationtPhaseTime;
}
......@@ -497,6 +676,12 @@ public class StateMulticopterMovement implements UAVMovementModel {
* @param position
*/
private void locationReached(PositionVector position) {
assert position == currentTargetPosition;
// remove current target position when reached
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