Commit de0604d3 authored by Julian Zobel's avatar Julian Zobel
Browse files

Merge remote-tracking branch 'origin/jz/movementmodel-testbed' into jz/dev-metrics

parents 82c106d1 4b1d6a8b
......@@ -21,12 +21,13 @@
package de.tud.kom.p2psim.api.topology.movement;
import de.tud.kom.p2psim.api.topology.component.ControllableLocationActuator;
import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
public interface SimUAVLocationActuator extends SimLocationActuator, ControllableLocationActuator {
public UAVMovementModel getUAVMovement();
public void setUAVMovement(UAVMovementModel uavMovement);
public ActuatorEnergyComponent getActuatorEnergyComponent();
}
......@@ -21,16 +21,21 @@
package de.tud.kom.p2psim.api.topology.movement;
import java.util.LinkedList;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
public interface UAVMovementModel {
public void setPreferredCruiseSpeed(double v_pref);
public void setMotorControl(StatelessMotorComponent motor);
public void setPreferredSpeed(double v_pref);
public double getMaxCruiseSpeed();
public double getMinCruiseSpeed();
public double verticalAscentMaxVelocity();
public double horizontalMaxVelocity();
public double getCurrentSpeed();
public double minimumVelocity();
public void move(long timeBetweenMovementOperations);
......
/*
* Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see <http://www.gnu.org/licenses/>.
*
*/
package de.tud.kom.p2psim.impl.energy.components;
import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;
public class MotorCharacteristic {
private final double thrust;
private final double current;
@XMLConfigurableConstructor({ "thrust", "current" })
public MotorCharacteristic(double thrust, double current) {
this.thrust = thrust;
this.current = current;
}
public double getCurrent() {
return current;
}
public double getThrust() {
return thrust;
}
@Override
public String toString() {
return "["+ thrust + "N @ " + current + "A]";
}
}
......@@ -36,7 +36,7 @@ import de.tudarmstadt.maki.simonstrator.api.Time;
* @author Julian Zobel
* @version 1.0, 11.09.2018
*/
public class ActuatorEnergyComponent implements EnergyComponent {
public class StateActuatorEnergyComponent implements EnergyComponent {
/**
* States supported by this energy component
......@@ -53,7 +53,7 @@ public class ActuatorEnergyComponent implements EnergyComponent {
private double actuatorLoad;
public ActuatorEnergyComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
public StateActuatorEnergyComponent(int numberOfActuators, double volt, double hoverAmp, double maxAmp) {
OFF = new DefaultEnergyState("OFF", 0);
FLY = new DefaultEnergyState("FLY", numberOfActuators * (hoverAmp * volt) * 1000000);
MAX = new DefaultEnergyState("MAX", numberOfActuators * (maxAmp * volt) * 1000000);
......
/*
* Copyright (c) 2005-2010 KOM � Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see <http://www.gnu.org/licenses/>.
*
*/
package de.tud.kom.p2psim.impl.energy.components;
import java.util.Comparator;
import java.util.LinkedList;
import de.tud.kom.p2psim.api.energy.ComponentType;
import de.tud.kom.p2psim.api.energy.EnergyComponent;
import de.tud.kom.p2psim.api.energy.EnergyEventListener;
import de.tud.kom.p2psim.api.energy.EnergyState;
import de.tud.kom.p2psim.impl.energy.DefaultEnergyState;
import de.tudarmstadt.maki.simonstrator.api.Time;
/**
* Component for devices that provide thrust, such as electrical motors for UAV propulsion.
* Is configured by {@link MotorCharacteristic}s with a given thrust (N) and a given current (A).
* Values in between given characteristics are calculated by linear interpolation.
*
*
* @author Julian Zobel
* @version 1.0, 05.03.2019
*/
public class StatelessMotorComponent implements EnergyComponent {
public enum componentState {OFF, ON}
private componentState state;
private EnergyEventListener energyModel;
private long lastEnergyConsumationTime;
private double volts;
private final double uJconversionFactor = 1000000;
private final int numberOfActuators;
private double thrust;
private double amps;
private EnergyState energyState;
private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
public StatelessMotorComponent(int numberOfActuators, double volt) {
this.volts = volt;
this.numberOfActuators = numberOfActuators;
this.state = componentState.OFF;
this.lastEnergyConsumationTime = Time.getCurrentTime();
thrust = 0;
amps = 0;
energyState = new DefaultEnergyState("OFF", 0);
}
/**
* Get the maximum thrust provided by this component.
* @return
*/
public double getMaxThrust() {
return characteristics.getLast().getThrust();
}
/**
* Set the new energy state and calculate the energy consumption from the last state
*/
private void setEnergyState() {
// set the new energy state
EnergyState newState;
if(state == componentState.ON) {
newState = new DefaultEnergyState("Actuator", numberOfActuators * (amps * volts) * uJconversionFactor);
}
else {
newState = new DefaultEnergyState("OFF", 0);
}
// calculate energy consumption for the previous state
long timeSpentInState = Time.getCurrentTime() - lastEnergyConsumationTime;
double cons = calculateEnergyConsumation(energyState, timeSpentInState);
System.out.println("consumed energy : " + (cons/uJconversionFactor) + " in " + Time.getFormattedTime(timeSpentInState));
energyModel.componentConsumedEnergy(this, cons);
// set new state
energyState = newState;
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
* @return The amount of thrust this component now generates.
*/
public double requestThrust(double targetThrust) {
if(targetThrust == 0 || targetThrust <= characteristics.getFirst().getThrust()) {
setLoad(characteristics.getFirst());
}
else if(targetThrust >= characteristics.getLast().getThrust()) {
setLoad(characteristics.getLast());
}
else {
approximateThrustCurrentRelation(targetThrust);
}
System.out.println("Now providing Thrust: " + this.thrust + " N");
return this.thrust;
}
/**
* Given an amount of thrust between the minimum and maximum values, the required current
* to provide this amount of thrust is calculated by linear interpolation by the nearest lower
* and upper {@link MotorCharacteristic}s.
*
* @param targetThrust
*/
private void approximateThrustCurrentRelation(double targetThrust) {
MotorCharacteristic lower = null, upper = null;
// find the lower and upper bounding characteristics
for (MotorCharacteristic ch : characteristics) {
//
if(ch.getThrust() == targetThrust) {
setLoad(ch);
return;
}
else {
// list is sorted, lower bound is the biggest that is lower
if(ch.getThrust() < targetThrust) {
lower = ch;
}
// the first that is greater is used as upper bound
else if(ch.getThrust() > targetThrust) {
upper = ch;
break;
}
}
}
if(upper == null || lower == null) {
throw new UnsupportedOperationException("Lower or upper bounds cannot be null");
}
if(upper.getThrust() < targetThrust || lower.getThrust() > targetThrust) {
throw new UnsupportedOperationException("Lower or upper bound do not match");
}
/*
* Calculate the approximated current with the upper and lower bounds:
* Amp_approx = Amp_lower + (T_target - T_lower)/(T_upper - T_lower) * (Amp_upper - Amp_lower)
*/
double delta = (targetThrust - lower.getThrust())/(upper.getThrust() - lower.getThrust());
double calculatedAmps = lower.getCurrent() + delta * (upper.getCurrent() - lower.getCurrent());
System.out.println(targetThrust + " N targeted => " + calculatedAmps + " A calculated | " + delta + " <> " + lower + " " + upper);
setLoad(targetThrust, calculatedAmps);
}
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();
}
/**
* Add a {@link MotorCharacteristic} for this motor.
*
* @param c
*/
public void addChar(MotorCharacteristic c) {
characteristics.add(c);
// sort the characteristics starting from low to high thrust
characteristics.sort(new Comparator<MotorCharacteristic>() {
@Override
public int compare(MotorCharacteristic o1,
MotorCharacteristic o2) {
return (int) (o1.getThrust() - o2.getThrust());
}
});
}
@Override
public void eventOccurred(Object content, int type) {
// TODO Auto-generated method stub
}
@Override
public ComponentType getType() {
return ComponentType.ACTUATOR;
}
@Override
public boolean turnOff() {
this.thrust = 0;
this.amps = 0;
this.state = componentState.OFF;
setEnergyState();
return true;
}
@Override
public boolean turnOn() {
if (isAvailable()) {
if(this.state != componentState.ON) {
this.state = componentState.ON;
requestThrust(0);
}
return true;
}
return false;
}
public boolean isAvailable() {
if (energyModel.componentCanBeActivated(this)) {
return true;
}
return false;
}
@Override
public boolean isOn() {
if(this.state != componentState.OFF && isAvailable()) {
return true;
}
return false;
}
@Override
public void setEnergyEventListener(EnergyEventListener listener) {
energyModel = listener;
}
public EnergyState getCurrentState() {
return energyState;
}
}
/*
* Copyright (c) 2005-2010 KOM � Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see <http://www.gnu.org/licenses/>.
*
*/
package de.tud.kom.p2psim.impl.energy.configs;
import java.util.LinkedList;
import de.tud.kom.p2psim.api.common.SimHost;
import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
import de.tud.kom.p2psim.impl.energy.components.MotorCharacteristic;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
/**
*
*
* @author Julian Zobel
* @version 1.0, 05.03.2019
*/
public class ActuatorConfiguration implements EnergyConfiguration<StatelessMotorComponent> {
private int numberOfActuators;
private double volt;
private LinkedList<MotorCharacteristic> characteristics = new LinkedList<>();
@Override
public StatelessMotorComponent getConfiguredEnergyComponent(SimHost host) {
StatelessMotorComponent comp = new StatelessMotorComponent(numberOfActuators, volt);
for (MotorCharacteristic c : characteristics) {
comp.addChar(c);
}
return comp;
}
@Override
public String getHelp() {
return "Fix actuator energy consumption config";
}
@Override
public boolean isWellConfigured() {
if(numberOfActuators >= 1 && volt > 0 && characteristics.size() >= 2) {
return true;
}
return false;
}
public void setNumberOfActuators(int num) {
numberOfActuators = num;
}
public void setVolt(double voltage) {
this.volt = voltage;
}
public void setMotorCharacteristic(MotorCharacteristic c) {
characteristics.add(c);
}
}
......@@ -22,15 +22,15 @@ package de.tud.kom.p2psim.impl.energy.configs;
import de.tud.kom.p2psim.api.common.SimHost;
import de.tud.kom.p2psim.api.energy.EnergyConfiguration;
import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
/**
* Energy Configuration for {@link ActuatorEnergyComponent}s.
* Energy Configuration for {@link StateActuatorEnergyComponent}s.
*
* @author Julian Zobel
* @version 1.0, 11.09.2018
*/
public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<ActuatorEnergyComponent> {
public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<StateActuatorEnergyComponent> {
private int numberOfActuators;
private double volt;
......@@ -38,8 +38,8 @@ public class ActuatorEnergyConsumptionConfig implements EnergyConfiguration<Actu
private double flyAmp; // in ampere
@Override
public ActuatorEnergyComponent getConfiguredEnergyComponent(SimHost host) {
return new ActuatorEnergyComponent(numberOfActuators, volt, hoverAmp, flyAmp);
public StateActuatorEnergyComponent getConfiguredEnergyComponent(SimHost host) {
return new StateActuatorEnergyComponent(numberOfActuators, volt, hoverAmp, flyAmp);
}
@Override
......
......@@ -25,7 +25,7 @@ import de.tud.kom.p2psim.api.energy.Battery;
import de.tud.kom.p2psim.api.energy.EnergyComponent;
import de.tud.kom.p2psim.api.energy.EnergyEventListener;
import de.tud.kom.p2psim.api.network.SimNetInterface;
import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
/**
* Energy Model based on multiple exchangeable components. Each component states the amount of consumed energy,
......@@ -60,8 +60,8 @@ public class ComponentBasedEnergyModel extends AbstractEnergyModel implements En
monitorEmptyBattery();
if(component instanceof ActuatorEnergyComponent) {
((ActuatorEnergyComponent) component).turnOff();
if(component instanceof StateActuatorEnergyComponent) {
((StateActuatorEnergyComponent) component).turnOff();
}
/*
......
......@@ -35,7 +35,8 @@ import de.tud.kom.p2psim.api.topology.movement.SimUAVLocationActuator;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.api.topology.placement.PlacementModel;
import de.tud.kom.p2psim.impl.energy.RechargeableBattery;
import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StateActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.energy.models.AbstractEnergyModel;
import de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
......@@ -64,7 +65,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
private OverlayComponent uavOverlayComponent;
private ActuatorEnergyComponent actuator;
private StatelessMotorComponent actuator;
private RechargeableBattery battery;
private UAVstate state = UAVstate.OFFLINE;
......@@ -90,7 +91,8 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
try {
actuator = getHost().getComponent(EnergyModel.class)
.getComponent(ComponentType.ACTUATOR, ActuatorEnergyComponent.class);
.getComponent(ComponentType.ACTUATOR, StatelessMotorComponent.class);
movement.setMotorControl(actuator);
} catch (ComponentNotAvailableException e) {
System.err.println("No Acutator Energy Component was found!");
}
......@@ -120,12 +122,12 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public double getMinMovementSpeed() {
return movement.getMinCruiseSpeed();
return movement.minimumVelocity();
}
@Override
public double getMaxMovementSpeed() {
return movement.getMaxCruiseSpeed();
return movement.horizontalMaxVelocity();
}
@Override
......@@ -135,7 +137,7 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
@Override
public void setMovementSpeed(double speed) {
movement.setPreferredCruiseSpeed(speed);
movement.setPreferredSpeed(speed);
}
@Override
......@@ -210,11 +212,6 @@ public class UAVTopologyComponent extends AbstractTopologyComponent implements S
this.movement = uavMovement;
}
@Override
public ActuatorEnergyComponent getActuatorEnergyComponent() {
return actuator;
}
@Override
public Set<AttractionPoint> getAllAttractionPoints() {
throw new UnsupportedOperationException();
......
/*
* Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see <http://www.gnu.org/licenses/>.
*
*/
package de.tud.kom.p2psim.impl.topology.movement.aerial;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.Map;
import org.apache.commons.math3.geometry.Vector;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
public class MulticopterMovement implements UAVMovementModel {
private UAVTopologyComponent topologyComponent;
private PositionVector direction;
private double currentAngleOfAttack;
private double currentSpeed;
private double preferredSpeed;
private LinkedList<PositionVector> route = new LinkedList<>();
private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>(); // TODO callback interface
private StatelessMotorComponent motor;
private double mass = 1.465; // kg
private final double airdensity = 1.2255; // kg/m^3
private final double gravity = 9.807; // m/s^2
private double A_top = 0.245; // m^2
private double A_front = 0.04; // m^2
private double dragCoefficient = 0.5;
private double maxPitchAngle = Math.toRadians(45); // 45° max angle
private double descentVelocityMax = 5; // m/s
private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
public MulticopterMovement(UAVTopologyComponent topologyComponent) {
this.topologyComponent = topologyComponent;
this.direction = new PositionVector(0,0);
}
/*
*
*/
public double verticalDescentMaxThrust() {
// m * g - 0.5 * p * C * A * v^2
return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * descentVelocityMax * descentVelocityMax;
}
public double verticalAscentMaxAcceleration() {
return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
}
public double verticalAscentMaxVelocity() {
double maxThrust = motor.getMaxThrust();
return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));
}
public double hoverThrustRequired() {
return mass * gravity;
}
public double horizontalMaxVelocity() {
double horizontalThrust = horizontalMaxThrust();
double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
return maxVelocity;
}
public double horizontalMaxThrust() {
// hoverthrust / cos => amount of thrust in horizonal direction with °angle
double stableAltitudeMaximumTotalThrust = hoverThrustRequired() / Math.cos(maxPitchAngle);
// fraction of total thrust in horizonal (forward) direction with °angle
double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
return maximumHorizontalThrustStableAltitude;
}
public double bodyDrag(double angleRadians, PositionVector direction) {
return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
}
public double areaExposedToDrag(double angleRadians, PositionVector direction) {
Vector2D v = new Vector2D(Math.abs(direction.getX()) + Math.abs(direction.getY()), Math.abs(direction.getZ()));
v = v.normalize();
double areaExposedFront = v.getX() * (Math.sin(angleRadians) * A_top + Math.cos(angleRadians) * A_front);
double areaExposedTop = v.getY() * (Math.cos(angleRadians) * A_top + Math.sin(angleRadians) * A_front);
return areaExposedFront + areaExposedTop;
}
/*
* F_drag [N] = 0.5 * p * C_drag * A * v^2
*/
public double currentDrag() {
return 0.5 * bodyDrag(currentAngleOfAttack, direction) * currentSpeed * currentSpeed;
}
/*
*
*/
@Override
public void setMotorControl(StatelessMotorComponent motor) {
this.motor = motor;
}
@Override
public void move(long timeBetweenMovementOperations) {
if(motor.isOn() && !route.isEmpty()) {
PositionVector position = new PositionVector(topologyComponent.getRealPosition());
double thrust = 0;
direction = new PositionVector(0,0,1);
double distz = 100 - position.getZ();
if(distz < 100) {
double time = ((double) timeBetweenMovementOperations) / ((double) Time.SECOND);
// ASCENSION TO SAFETY HEIGHT
thrust = motor.requestThrust(motor.getMaxThrust());
double acc = ((thrust - hoverThrustRequired()) / mass ) - currentDrag();
double speed = 0;
if(acc < 0.1) {
// normal flight
speed = verticalAscentMaxVelocity();
}
else {
// acceleration
speed = currentSpeed + acc * time;
}
System.out.println("ACCELERATION : " + acc);
System.out.println("SPEED : " + speed);
//double dist = 0.5 * acc * (timeBetweenMovementOperations / Time.SECOND) * (timeBetweenMovementOperations / Time.SECOND);
double dist = currentSpeed * time;
//double safetydistance = speed
//System.out.println();
position.add(new PositionVector(0,0,dist));
System.out.println("NEW POSITION : " + position);
topologyComponent.updateCurrentLocation(position);
}
else {
thrust = motor.requestThrust(hoverThrustRequired());
topologyComponent.updateCurrentLocation(position);
}
// PositionVector directionToTarget = new PositionVector(route.getFirst());
// directionToTarget.subtract(position);
//
// double distanceToTargetPosition = directionToTarget.getLength();
// // If target point is reached within a 1 meter margin, we remove that point from the list
// if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
// {
// route.removeFirst();
//
// topologyComponent.updateCurrentLocation(position);
// currentSpeed = 0;
//
// locationReached(position);
// return;
// }
//
// double timefactor = timeBetweenMovementOperations / Time.SECOND;
//
// currentSpeed = Math.min(distanceToTargetPosition, preferredSpeed);
//
// direction.normalize();
// direction.multiplyScalar(currentSpeed * timefactor);
//
// PositionVector newPosition = new PositionVector(position);
// newPosition.add(direction);
//
// topologyComponent.updateCurrentLocation(newPosition);
}
//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " + topologyComponent);
}
@Override
public void setPreferredSpeed(double v_pref) {
this.preferredSpeed = v_pref;
}
@Override
public double getCurrentSpeed() {
return currentSpeed;
}
/**
* Trigger the callback function, if there is a valid callback
*
* @param position
*/
private void locationReached(PositionVector position) {
if(locationCallbacks.containsKey(position)) {
locationCallbacks.get(position).reachedLocation();
}
}
@Override
public void setTargetLocation(PositionVector target,
ReachedLocationCallback reachedLocationCallback) {
route.clear();
route.add(target);
if(reachedLocationCallback != null)
locationCallbacks.put(target, reachedLocationCallback);
}
@Override
public void setTargetLocationRoute(LinkedList<PositionVector> route,
ReachedLocationCallback reachedLocationCallback) {
this.route.clear();
this.route.addAll(route);
if(reachedLocationCallback != null)
locationCallbacks.put(route.getLast(), reachedLocationCallback);
}
@Override
public void addTargetLocation(PositionVector target,
ReachedLocationCallback reachedLocationCallback) {
route.add(target);
if(reachedLocationCallback != null)
locationCallbacks.put(target, reachedLocationCallback);
}
@Override
public LinkedList<PositionVector> getTargetLocations() {
LinkedList<PositionVector> copy = new LinkedList<>();
for (PositionVector pv : route) {
copy.add(pv.clone());
}
return copy;
}
@Override
public void removeTargetLocations() {
route.clear();
locationCallbacks.clear();
}
@Override
public double minimumVelocity() {
return 0;
}
}
......@@ -28,6 +28,7 @@ import java.util.Map;
import org.joda.time.tz.ZoneInfoProvider;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
......@@ -52,6 +53,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
private LinkedList<PositionVector> route = new LinkedList<>();
private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>(); // TODO callback interface
private StatelessMotorComponent motor;
public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
this.topologyComponent = topologyComponent;
......@@ -62,18 +64,13 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
}
@Override
public void setPreferredCruiseSpeed(double v_pref) {
this.preferredCruiseSpeed = v_pref;
}
@Override
public double getMaxCruiseSpeed() {
return maxCruiseSpeed;
public void setMotorControl(StatelessMotorComponent motor) {
this.motor = motor;
}
@Override
public double getMinCruiseSpeed() {
return minCruiseSpeed;
public void setPreferredSpeed(double v_pref) {
this.preferredCruiseSpeed = v_pref;
}
@Override
......@@ -96,10 +93,10 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
{
route.removeFirst();
topologyComponent.getActuatorEnergyComponent().useActuator(0.5);
motor.requestThrust(16.3);
topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
currentSpeed = 0;
topologyComponent.getActuatorEnergyComponent().useActuator(0); // now hover
motor.requestThrust(14.4); // now hover
locationReached(targetPosition);
return;
}
......@@ -116,7 +113,7 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
PositionVector newPosition = new PositionVector(currentPosition);
newPosition.add(direction);
topologyComponent.getActuatorEnergyComponent().useActuator(1);
motor.requestThrust(22);
topologyComponent.updateCurrentLocation(newPosition);
}
......@@ -179,6 +176,21 @@ public class SimpleMutlicopterMovement implements UAVMovementModel {
locationCallbacks.clear();
}
@Override
public double verticalAscentMaxVelocity() {
return maxCruiseSpeed;
}
@Override
public double horizontalMaxVelocity() {
return maxCruiseSpeed;
}
@Override
public double minimumVelocity() {
return 0;
}
......
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