diff --git a/org.fortiss.af3.platform.raspberry.ui/.gitignore b/org.fortiss.af3.platform.raspberry.ui/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..237ede120c79c583d7292bd0e94832d2b863ce18
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry.ui/.gitignore
@@ -0,0 +1,2 @@
+build/
+target/
diff --git a/org.fortiss.af3.platform.raspberry.ui/icons/console.png b/org.fortiss.af3.platform.raspberry.ui/icons/console.png
new file mode 100644
index 0000000000000000000000000000000000000000..5cf0d0e5de7790df73b28f4c7ee8a27382541f0f
Binary files /dev/null and b/org.fortiss.af3.platform.raspberry.ui/icons/console.png differ
diff --git a/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/CameraInputHandler.java b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/CameraInputHandler.java
new file mode 100644
index 0000000000000000000000000000000000000000..d10c0ac65763baadf88f442a5542980248abba4e
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/CameraInputHandler.java
@@ -0,0 +1,42 @@
+/*--------------------------------------------------------------------------+
+$Id: CameraInputHandler.java 23956 2018-04-11 14:15:25Z barner $
+| |
+| Copyright 2011 ForTISS GmbH |
+| |
+| Licensed under the Apache License, Version 2.0 (the "License"); |
+| you may not use this file except in compliance with the License. |
+| You may obtain a copy of the License at |
+| |
+| http://www.apache.org/licenses/LICENSE-2.0 |
+| |
+| Unless required by applicable law or agreed to in writing, software |
+| distributed under the License is distributed on an "AS IS" BASIS, |
+| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
+| See the License for the specific language governing permissions and |
+| limitations under the License. |
++--------------------------------------------------------------------------*/
+package org.fortiss.af3.platform.raspberry.ui.handler;
+
+import static org.fortiss.af3.platform.raspberry.ui.AF3PlatformRaspberryUIActivator.getImageDescriptor;
+
+import org.eclipse.jface.resource.ImageDescriptor;
+import org.fortiss.af3.platform.raspberry.model.RaspberryPiReceiverBase;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraDistanceLeft;
+import org.fortiss.tooling.kernel.ui.extension.base.NamedCommentedModelElementHandlerBase;
+
+/**
+ * Handler for {@link CameraDistanceLeft}s and other camera model elements.
+ *
+ * @author hoelzl
+ * @author $Author: barner $
+ * @version $Rev: 23956 $
+ * @ConQAT.Rating GREEN Hash: D80CF94CBBADB4627AAEDA1F34ABF0A5
+ */
+public class CameraInputHandler extends
+ NamedCommentedModelElementHandlerBase<RaspberryPiReceiverBase> {
+ /** {@inheritDoc} */
+ @Override
+ public ImageDescriptor getIconImageDescriptor() {
+ return getImageDescriptor("icons/unknown.png");
+ }
+}
diff --git a/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlInputHandler.java b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlInputHandler.java
new file mode 100644
index 0000000000000000000000000000000000000000..5b594be50d91ba2c4c1d3c7ba6c4cf311c389239
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlInputHandler.java
@@ -0,0 +1,41 @@
+/*--------------------------------------------------------------------------+
+$Id: MotorControlInputHandler.java 23956 2018-04-11 14:15:25Z barner $
+| |
+| Copyright 2011 ForTISS GmbH |
+| |
+| Licensed under the Apache License, Version 2.0 (the "License"); |
+| you may not use this file except in compliance with the License. |
+| You may obtain a copy of the License at |
+| |
+| http://www.apache.org/licenses/LICENSE-2.0 |
+| |
+| Unless required by applicable law or agreed to in writing, software |
+| distributed under the License is distributed on an "AS IS" BASIS, |
+| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
+| See the License for the specific language governing permissions and |
+| limitations under the License. |
++--------------------------------------------------------------------------*/
+package org.fortiss.af3.platform.raspberry.ui.handler;
+
+import static org.fortiss.af3.platform.raspberry.ui.AF3PlatformRaspberryUIActivator.getImageDescriptor;
+
+import org.eclipse.jface.resource.ImageDescriptor;
+import org.fortiss.af3.platform.raspberry.model.motorcontrol.MotorControlInput;
+import org.fortiss.tooling.kernel.ui.extension.base.NamedCommentedModelElementHandlerBase;
+
+/**
+ * Handler for {@link MotorControlInput}s.
+ *
+ * @author hoelzl
+ * @author $Author: barner $
+ * @version $Rev: 23956 $
+ * @ConQAT.Rating GREEN Hash: D80CF94CBBADB4627AAEDA1F34ABF0A5
+ */
+public class MotorControlInputHandler extends
+ NamedCommentedModelElementHandlerBase<MotorControlInput> {
+ /** {@inheritDoc} */
+ @Override
+ public ImageDescriptor getIconImageDescriptor() {
+ return getImageDescriptor("icons/unknown.png");
+ }
+}
diff --git a/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlOutputHandler.java b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlOutputHandler.java
new file mode 100644
index 0000000000000000000000000000000000000000..d5e36ceb79008e523ee096d8e38d3741daccc181
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry.ui/src/org/fortiss/af3/platform/raspberry/ui/handler/MotorControlOutputHandler.java
@@ -0,0 +1,41 @@
+/*--------------------------------------------------------------------------+
+$Id: MotorControlOutputHandler.java 23956 2018-04-11 14:15:25Z barner $
+| |
+| Copyright 2011 ForTISS GmbH |
+| |
+| Licensed under the Apache License, Version 2.0 (the "License"); |
+| you may not use this file except in compliance with the License. |
+| You may obtain a copy of the License at |
+| |
+| http://www.apache.org/licenses/LICENSE-2.0 |
+| |
+| Unless required by applicable law or agreed to in writing, software |
+| distributed under the License is distributed on an "AS IS" BASIS, |
+| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
+| See the License for the specific language governing permissions and |
+| limitations under the License. |
++--------------------------------------------------------------------------*/
+package org.fortiss.af3.platform.raspberry.ui.handler;
+
+import static org.fortiss.af3.platform.raspberry.ui.AF3PlatformRaspberryUIActivator.getImageDescriptor;
+
+import org.eclipse.jface.resource.ImageDescriptor;
+import org.fortiss.af3.platform.raspberry.model.motorcontrol.MotorControlOutput;
+import org.fortiss.tooling.kernel.ui.extension.base.NamedCommentedModelElementHandlerBase;
+
+/**
+ * Handler for {@link MotorControlOutput}s.
+ *
+ * @author hoelzl
+ * @author $Author: barner $
+ * @version $Rev: 23956 $
+ * @ConQAT.Rating GREEN Hash: D80CF94CBBADB4627AAEDA1F34ABF0A5
+ */
+public class MotorControlOutputHandler extends
+ NamedCommentedModelElementHandlerBase<MotorControlOutput> {
+ /** {@inheritDoc} */
+ @Override
+ public ImageDescriptor getIconImageDescriptor() {
+ return getImageDescriptor("icons/unknown.png");
+ }
+}
diff --git a/org.fortiss.af3.platform.raspberry/.gitignore b/org.fortiss.af3.platform.raspberry/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..237ede120c79c583d7292bd0e94832d2b863ce18
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry/.gitignore
@@ -0,0 +1,2 @@
+build/
+target/
diff --git a/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/executable/MultiUnitMainGenerator.java b/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/executable/MultiUnitMainGenerator.java
new file mode 100644
index 0000000000000000000000000000000000000000..c8df8b528c187f324db7d145178b2993d5225a94
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/executable/MultiUnitMainGenerator.java
@@ -0,0 +1,1060 @@
+/*--------------------------------------------------------------------------+
+$Id: MultiUnitMainGenerator.java 23075 2018-02-01 14:25:45Z ext.konstantin.tschernik $
+| |
+| Copyright 2017 fortiss GmbH |
+| |
+| Licensed under the Apache License, Version 2.0 (the "License"); |
+| you may not use this file except in compliance with the License. |
+| You may obtain a copy of the License at |
+| |
+| http://www.apache.org/licenses/LICENSE-2.0 |
+| |
+| Unless required by applicable law or agreed to in writing, software |
+| distributed under the License is distributed on an "AS IS" BASIS, |
+| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
+| See the License for the specific language governing permissions and |
+| limitations under the License. |
++--------------------------------------------------------------------------*/
+package org.fortiss.af3.platform.raspberry.generator.executable;
+
+import static org.fortiss.af3.component.utils.ComponentArchitectureUtils.findSourcePort;
+import static org.fortiss.af3.component.utils.ComponentArchitectureUtils.isAtomicComponent;
+import static org.fortiss.af3.deployment.utils.DeploymentUtils.findDeployedExecutionUnits;
+import static org.fortiss.af3.platform.raspberry.generator.templates.RasPiCTemplates.getMultiUnitMainCFile;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.LinkedList;
+import java.util.List;
+
+import org.conqat.lib.commons.collections.Pair;
+import org.fortiss.af3.component.model.Channel;
+import org.fortiss.af3.component.model.Component;
+import org.fortiss.af3.component.model.InputPort;
+import org.fortiss.af3.component.model.OutputPort;
+import org.fortiss.af3.component.model.Port;
+import org.fortiss.af3.deployment.model.Deployment;
+import org.fortiss.af3.expression.model.definitions.Enumeration;
+import org.fortiss.af3.expression.model.definitions.TypeDefinition;
+import org.fortiss.af3.expression.model.types.TBool;
+import org.fortiss.af3.expression.model.types.TDefinedType;
+import org.fortiss.af3.expression.model.types.TDouble;
+import org.fortiss.af3.expression.model.types.TInt;
+import org.fortiss.af3.generator.common.model.source.AbstractUnit;
+import org.fortiss.af3.platform.model.ExecutionUnit;
+import org.fortiss.af3.platform.model.PlatformConnectorUnit;
+import org.fortiss.af3.platform.model.Receiver;
+import org.fortiss.af3.platform.model.Transmitter;
+import org.fortiss.af3.platform.raspberry.model.ActuatorPWM;
+import org.fortiss.af3.platform.raspberry.model.ConsoleOutput;
+import org.fortiss.af3.platform.raspberry.model.RaspberryPi;
+import org.fortiss.af3.platform.raspberry.model.brick.AccelerationXSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.AccelerationYSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.AccelerationZSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.ActuatorDigits;
+import org.fortiss.af3.platform.raspberry.model.brick.AngularVelocityXSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.AngularVelocityYSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.AngularVelocityZSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.LaserRangeSensor;
+import org.fortiss.af3.platform.raspberry.model.brick.UltraSonicSensor;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraDetectionStateLeft;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraDetectionStateRight;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraDistanceLeft;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraDistanceRight;
+import org.fortiss.af3.platform.raspberry.model.camera.CameraYawAngle;
+import org.fortiss.af3.platform.raspberry.model.gamepad.Button1;
+import org.fortiss.af3.platform.raspberry.model.gamepad.Button2;
+import org.fortiss.af3.platform.raspberry.model.gamepad.Button3;
+import org.fortiss.af3.platform.raspberry.model.gamepad.Button4;
+import org.fortiss.af3.platform.raspberry.model.gamepad.ButtonL2;
+import org.fortiss.af3.platform.raspberry.model.gamepad.ButtonR2;
+import org.fortiss.af3.platform.raspberry.model.gamepad.GamepadReceiverBase;
+import org.fortiss.af3.platform.raspberry.model.motorcontrol.MotorControlInput;
+import org.fortiss.af3.platform.raspberry.model.motorcontrol.MotorControlOutput;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonA;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonB;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonHome;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonSelect;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonStart;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonX;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonY;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.DPadDown;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.DPadLeft;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.DPadRight;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.DPadUp;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.L2_Position;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.R2_Position;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.RumbleMagnitudeStrong;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.RumbleMagnitudeWeak;
+import org.fortiss.af3.platform.raspberry.model.rumblepad.RumblepadReceiverBase;
+import org.fortiss.af3.project.model.typesystem.IType;
+import org.fortiss.tooling.kernel.extension.data.ITransformationContext;
+import org.fortiss.tooling.kernel.model.INamedElement;
+
+/**
+ * Separate class for generating the main.c file for use with CAN-based multi-unit deployments.
+ *
+ * @author hoelzl
+ * @author $Author: ext.konstantin.tschernik $
+ * @version $Rev: 23075 $
+ * @ConQAT.Rating RED Hash:
+ */
+class MultiUnitMainGenerator {
+ private static final int WAITING_SLEEP_IN_MICROS = 50;
+ private Deployment deployment;
+ private CanTransmissionCatalog catalog;
+ private RaspberryPi executionUnit;
+ private List<Component> atomicComponents;
+ private List<Pair<ExecutionUnit, Component>> deployedComponents;
+ private List<Pair<PlatformConnectorUnit, Port>> deployedPorts;
+ private HashMap<InputPort, Receiver> atomicInputToReceiver = new HashMap<>();
+ private HashMap<OutputPort, Transmitter> atomicOutputToTransmitter = new HashMap<>();
+ private ITransformationContext context;
+
+ private boolean useCamera;
+ private boolean useGamepad;
+ private boolean useRumblepad;
+ private boolean useDigits;
+ private boolean usePWM;
+ private boolean useUS;
+ private boolean useLaser;
+ private boolean useACC;
+ private boolean useConsole;
+ private boolean useRumble;
+ private OutputPort rumbleWeakPort = null;
+ private OutputPort rumbleStrongPort = null;
+ private boolean useVesc;
+ private int nrOfUsSensors = 0;
+
+ /** Constructor. */
+ public MultiUnitMainGenerator(Deployment deployment, CanTransmissionCatalog catalog,
+ RaspberryPi executionUnit, List<Component> atomicComponents,
+ List<Pair<ExecutionUnit, Component>> deployedComponents,
+ List<Pair<PlatformConnectorUnit, Port>> deployedPorts, ITransformationContext context) {
+ this.deployment = deployment;
+ this.catalog = catalog;
+ this.executionUnit = executionUnit;
+ this.deployedComponents = deployedComponents;
+ this.atomicComponents = atomicComponents;
+ this.deployedPorts = deployedPorts;
+ this.context = context;
+ for(Pair<PlatformConnectorUnit, Port> p : deployedPorts) {
+ PlatformConnectorUnit connector = p.getFirst();
+ analyzeDeployedPort(connector);
+ if(connector instanceof Receiver && p.getSecond() instanceof InputPort) {
+ InputPort deployedInput = (InputPort)p.getSecond();
+ for(Port atomicPort : findTargetPorts(deployedInput.getComponent(), deployedInput)) {
+ if(atomicPort instanceof InputPort) {
+ atomicInputToReceiver.put((InputPort)atomicPort, (Receiver)connector);
+ }
+ }
+ } else if(connector instanceof Transmitter && p.getSecond() instanceof OutputPort) {
+ OutputPort deployedOutput = (OutputPort)p.getSecond();
+ Port atomicPort = findSourcePort(deployedOutput.getComponent(), deployedOutput);
+ if(atomicPort instanceof OutputPort) {
+ atomicOutputToTransmitter.put((OutputPort)atomicPort, (Transmitter)connector);
+ }
+ }
+ }
+ }
+
+ /** Analyzes which {@link Receiver}s and {@link Transmitter}s are used. */
+ private void analyzeDeployedPort(PlatformConnectorUnit p) {
+ if(!useGamepad && p instanceof GamepadReceiverBase) {
+ useGamepad = true;
+ }
+ if(!useRumblepad &&
+ (p instanceof RumblepadReceiverBase || p instanceof RumbleMagnitudeWeak || p instanceof RumbleMagnitudeStrong)) {
+ useRumblepad = true;
+ }
+ if(!usePWM && p instanceof ActuatorPWM) {
+ usePWM = true;
+ }
+ if(!useUS && p instanceof UltraSonicSensor) {
+ useUS = true;
+ }
+ if(!useLaser && p instanceof LaserRangeSensor) {
+ useLaser = true;
+ }
+ if(!useDigits && p instanceof ActuatorDigits) {
+ useDigits = true;
+ }
+ if(!useACC &&
+ (p instanceof AccelerationXSensor || p instanceof AccelerationYSensor ||
+ p instanceof AccelerationZSensor || p instanceof AngularVelocityXSensor ||
+ p instanceof AngularVelocityYSensor || p instanceof AngularVelocityZSensor)) {
+ useACC = true;
+ }
+ if(!useConsole && p instanceof ConsoleOutput) {
+ useConsole = true;
+ }
+ if(!useVesc && (p instanceof MotorControlOutput || p instanceof MotorControlInput)) {
+ useVesc = true;
+ }
+ if(!useCamera &&
+ (p instanceof CameraYawAngle || p instanceof CameraDistanceLeft ||
+ p instanceof CameraDistanceRight || p instanceof CameraDetectionStateLeft) ||
+ p instanceof CameraDetectionStateRight) {
+ useCamera = true;
+ }
+ }
+
+ /** Creates the main.c file for deployments with a single execution units. */
+ public AbstractUnit createMultiUnitMain() {
+ String remoteUnitSetupCode = createRemoteUnitSetupCode();
+ String systemIncludes = createSystemIncludes();
+ String sensorVariables = createSensorVariables(deployedComponents, deployedPorts);
+ String systemInitCode = createInitCode(deployedComponents);
+ String remotePorts = createPortBuffers();
+ String componentSetup = createComponentStructureCode();
+ String componentImpl = createComponentImpl();
+
+ return getMultiUnitMainCFile(executionUnit.getName(), executionUnit.isCoordinatorUnit(),
+ executionUnit.getCanCoordinationID(), sensorVariables, systemInitCode,
+ remoteUnitSetupCode, systemIncludes, remotePorts, componentSetup, componentImpl,
+ executionUnit.getCycleTime(), WAITING_SLEEP_IN_MICROS);
+ }
+
+ /** Creates the C code for in-memory model of the AF3 component architecture. */
+ private String createComponentStructureCode() {
+ StringBuilder sb = new StringBuilder();
+ sb.append("// Instantiate components\n");
+ for(Component c : atomicComponents) {
+ createComponentInstantiationCode(sb, c);
+ }
+ instantiateOutputPorts(sb);
+ instantiateAndLinkInputPorts(sb);
+ return sb.toString();
+ }
+
+ /** Creates code for output ports. */
+ private void instantiateOutputPorts(StringBuilder sb) {
+ for(Component c : atomicComponents) {
+ String compId = identifierName(c);
+ sb.append("// Output ports of ").append(c.getName()).append("\n");
+ for(OutputPort outport : c.getOutputPorts()) {
+ if(atomicOutputToTransmitter.containsKey(outport)) {
+ // deployed to hardware
+ // FIXME: an atomic output might be needed remotely and on HW
+ continue;
+ }
+ String portVar = af3PortName(outport);
+ sb.append("\t").append(portVar).append(" = af3_port_create(");
+ sb.append(outport.getId()).append(", \"").append(outport.getName());
+ sb.append("\", ").append(getTypeName(outport)).append(", ");
+ boolean needsTransmission = catalog.isOutputNeededRemotely(outport);
+ sb.append(needsTransmission ? "true" : "false");
+ sb.append(");\n");
+ sb.append("\taf3_component_add_output(inst_").append(compId);
+ sb.append(", ").append(portVar).append(");\n");
+ sb.append("\tcopy_to_output_port_").append(getTypePostfix(outport)).append("(&")
+ .append(novalPortName(outport));
+ sb.append(", &").append(identifierName(outport));
+ sb.append(", ").append(portVar).append(");\n");
+ }
+ }
+ }
+
+ /** Returns the postfix for port copy functions of the given port. */
+ private Object getTypePostfix(Port port) {
+ IType type = port.getPortSpecification().getType();
+ if(type instanceof TInt) {
+ return "int";
+ }
+ if(type instanceof TDouble) {
+ return "double";
+ }
+ if(type instanceof TBool) {
+ return "boolean";
+ }
+ if(type instanceof TDefinedType) {
+ TypeDefinition tdef = ((TDefinedType)type).getDef();
+ if(tdef instanceof Enumeration) {
+ return "int";
+ }
+ }
+ return "unsupported_type";
+ }
+
+ /** Returns the type name of the given port. */
+ private Object getTypeName(Port port) {
+ IType type = port.getPortSpecification().getType();
+ if(type instanceof TInt) {
+ return "TYPE_INTEGER";
+ }
+ if(type instanceof TDouble) {
+ return "TYPE_DOUBLE";
+ }
+ if(type instanceof TBool) {
+ return "TYPE_BOOLEAN";
+ }
+ if(type instanceof TDefinedType) {
+ TypeDefinition tdef = ((TDefinedType)type).getDef();
+ if(tdef instanceof Enumeration) {
+ return "TYPE_INTEGER";
+ }
+ }
+ return "UNSUPPORTED_TYPE";
+ }
+
+ /** Creates remote input port proxies, input port buffers and links channels. */
+ private void instantiateAndLinkInputPorts(StringBuilder sb) {
+ // set for avoiding duplicate remote output port instantiation
+ HashSet<OutputPort> remoteOutputs = new HashSet<>();
+ for(Component c : atomicComponents) {
+ sb.append("// Local and remote input ports and channels of ").append(c.getName())
+ .append("\n");
+ for(InputPort inport : c.getInputPorts()) {
+ if(atomicInputToReceiver.containsKey(inport)) {
+ // HW allocation => no af3_port needed
+ continue;
+ }
+ String inPortName = af3PortName(inport);
+ sb.append("\t").append(inPortName).append(" = af3_port_create(");
+ sb.append(inport.getId()).append(", \"").append(inport.getName()).append("\", ");
+ sb.append(getTypeName(inport)).append(", false);\n");
+ sb.append("\taf3_component_add_input(inst_").append(identifierName(c));
+ sb.append(", ").append(inPortName).append(");\n");
+
+ OutputPort sourceOut = catalog.getSourcePort(inport);
+ // handle remote or local channel creation
+ if(catalog.hasRemoteSource(inport)) {
+ if(!remoteOutputs.contains(sourceOut)) {
+ createRemoteProxy(sb, sourceOut);
+ remoteOutputs.add(sourceOut);
+ }
+ }
+ if(sourceOut != null) {
+ createChannel(sb, sourceOut, inport);
+ }
+ }
+ }
+ }
+
+ /** Creates a channel between the given ports. */
+ private void createChannel(StringBuilder sb, OutputPort outport, InputPort inport) {
+ sb.append("\taf3_channel_create(").append(inport.getId());
+ sb.append(", \"ChannelTo").append(inport.getName()).append("\", ");
+ sb.append(af3PortName(outport)).append(", ");
+ sb.append(af3PortName(inport)).append(");\n");
+ }
+
+ /** Creates the remote input instantiation code. */
+ private void createRemoteProxy(StringBuilder sb, OutputPort remoteOut) {
+ // remote outputs instantiated only once
+ RaspberryPi owner = catalog.getRaspberryPi(remoteOut);
+ sb.append("\taf3_port_t* ").append(af3PortName(remoteOut));
+ sb.append(" = af3_component_remote_proxy_input_create(");
+ sb.append(catalog.getCanId(owner)).append(", ").append(remoteOut.getId());
+ sb.append(", \"").append(remoteOut.getName()).append("\", ");
+ sb.append(getTypeName(remoteOut)).append(");\n");
+ }
+
+ /** Creates code for instantiating components. */
+ private void createComponentInstantiationCode(StringBuilder sb, Component c) {
+ String compId = identifierName(c);
+ RaspberryPi owner = catalog.getRaspberryPi(c);
+ sb.append("// Instantiation of ").append(c.getName()).append("\n");
+ sb.append("\taf3_component_t* inst_").append(compId).append(" = af3_component_create(");
+ sb.append(catalog.getCanId(owner)).append(", \"").append(c.getName()).append("\", ");
+ sb.append(c.isStronglyCausal() ? "false" : "true");
+ sb.append(", impl_").append(compId).append(");\n");
+ sb.append("\taf3_component_system_add(inst_").append(compId).append(");\n");
+ sb.append("\tinit_").append(compId).append("();\n");
+ }
+
+ /** Creates static functions for each component. */
+ private String createComponentImpl() {
+ StringBuilder sb = new StringBuilder();
+ for(Component atomic : atomicComponents) {
+ createComponentImpl(atomic, sb);
+ }
+ return sb.toString();
+ }
+
+ /** Creates the component implementation code for the given component. */
+ private void createComponentImpl(Component atomic, StringBuilder sb) {
+ String compId = identifierName(atomic);
+ sb.append("// Implementation of ").append(atomic.getName()).append("\n");
+ sb.append("static void impl_").append(compId).append("(af3_component_t* comp) {\n");
+ if(useUS || useLaser || useACC) {
+ // Get Current time for the sensors
+ sb.append("uint64_t curr_time = time_util_get_current_micros();\n");
+ }
+ for(InputPort inport : atomic.getInputPorts()) {
+ if(atomicInputToReceiver.containsKey(inport)) {
+ // local hardware input ports
+ Receiver rec = atomicInputToReceiver.get(inport);
+ sb.append(createReadCode(rec, inport));
+ } else {
+ // local remote input ports
+ // FIXME: port type
+ sb.append("\tcopy_from_input_port_").append(getTypePostfix(inport));
+ sb.append("(").append(af3PortName(inport));
+ sb.append(", &").append(novalPortName(inport));
+ sb.append(", &").append(identifierName(inport)).append(");\n");
+ }
+ }
+ sb.append("\tperform_step_").append(compId).append("();\n");
+ useRumble = false;
+ for(OutputPort outport : atomic.getOutputPorts()) {
+ if(atomicOutputToTransmitter.containsKey(outport)) {
+ // local hardware output ports
+ Transmitter trn = atomicOutputToTransmitter.get(outport);
+ sb.append(createWriteCode(trn, outport));
+ } else {
+ // local or remote output port
+ // FIXME: port type
+ sb.append("\tcopy_to_output_port_int(&").append(novalPortName(outport));
+ sb.append(", &").append(identifierName(outport));
+ sb.append(", ").append(af3PortName(outport)).append(");\n");
+ }
+ }
+ if(useRumble) {
+ sb.append(createRumbleCode());
+ }
+ sb.append("}\n\n");
+ }
+
+ /** Setup code for remote units (used only in coordinator mode). */
+ private String createRemoteUnitSetupCode() {
+ if(!executionUnit.isCoordinatorUnit()) {
+ return "";
+ }
+ StringBuilder sb = new StringBuilder();
+ for(ExecutionUnit ecu : findDeployedExecutionUnits(deployment)) {
+ if(ecu != executionUnit && ecu instanceof RaspberryPi) {
+ RaspberryPi raspi = (RaspberryPi)ecu;
+ String idName = identifierName(raspi);
+ sb.append("protocol_can_remote_unit_config_t* ");
+ sb.append(idName).append(" = protocol_remote_unit_config_create(\"");
+ sb.append(idName).append("\", ");
+ sb.append(raspi.getCanCoordinationID()).append(");\n");
+ sb.append("list_util_append(remote_units, ");
+ sb.append(idName).append(");\n");
+
+ }
+ }
+ return sb.toString();
+ }
+
+ /** Creates declarations of remote unit buffers. */
+ private String createPortBuffers() {
+ StringBuilder sb = new StringBuilder();
+ for(Component atm : atomicComponents) {
+ for(InputPort inport : atm.getInputPorts()) {
+ if(!atomicInputToReceiver.containsKey(inport)) {
+ sb.append("static af3_port_t* " + af3PortName(inport) + " = NULL;\n");
+ }
+ }
+ for(OutputPort outport : atm.getOutputPorts()) {
+ if(!atomicOutputToTransmitter.containsKey(outport)) {
+ sb.append("static af3_port_t* " + af3PortName(outport) + " = NULL;\n");
+ }
+ }
+ }
+ return sb.toString();
+ }
+
+ /** Returns port name with 'af3_port_' prefix. */
+ private String af3PortName(Port port) {
+ return "af3_port_" + identifierName(port);
+ }
+
+ /** Returns port name with 'noval_' prefix. */
+ private String novalPortName(Port port) {
+ return "noval_" + identifierName(port);
+ }
+
+ /** Create #include for component architecture code. */
+ private String createSystemIncludes() {
+ StringBuilder sb = new StringBuilder();
+ for(Component atomic : atomicComponents) {
+ sb.append("#include \"" + identifierName(atomic) + ".h\"\n");
+ }
+ if(useCamera) {
+ sb.append("#include <camera_client.h>\n");
+ }
+ if(useGamepad) {
+ sb.append("#include <gamepad.h>\n");
+ }
+ if(useRumblepad) {
+ sb.append("#include <rumblepad.h>\n");
+ }
+ if(usePWM) {
+ sb.append("#include <temp_actuator.h>\n");
+ }
+ if(useUS || useLaser || useDigits || useACC) {
+ sb.append("#include <ip_connection.h>\n");
+ }
+ if(useUS) {
+ sb.append("#include <bricklet_distance_us.h>\n");
+ }
+ if(useLaser) {
+ sb.append("#include <bricklet_laser_range_finder.h>\n");
+ }
+ if(useACC) {
+ sb.append("#include <brick_imu_v2.h>\n");
+ }
+ if(useDigits) {
+ sb.append("#include <bricklet_segment_display_4x7.h>\n");
+ }
+ if(useVesc) {
+ sb.append("#include <commands.h>\n");
+ sb.append("#include <hal.h>\n");
+ }
+ sb.append("#include <stdio.h>\n");
+ return sb.toString();
+ }
+
+ /** Create the Sensor Variables and functions. */
+ private String createSensorVariables(List<Pair<ExecutionUnit, Component>> deployedComponents,
+ List<Pair<PlatformConnectorUnit, Port>> deployedPorts) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float Q = 0.022;\n");
+ sb.append("float R = 0.917;\n");
+ sb.append("float estimates[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};\n");
+ sb.append("float last_p[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};\n");
+ sb.append("float kalman_filter(float value, int sensor) {\n");
+ sb.append("\tif(sensor < 0 || sensor > 8) return -1;\n");
+ sb.append("\tfloat temp_est = estimates[sensor];\n");
+ sb.append("\tfloat p_temp = last_p[sensor] + Q;\n");
+ sb.append("\tfloat K = p_temp * (1.0*(p_temp + R));\n");
+ sb.append("\tfloat est = temp_est + K * (value - temp_est);\n");
+ sb.append("\tfloat P = (1-K)*p_temp;\n");
+ sb.append("\tlast_p[sensor] = P;\n");
+ sb.append("\testimates[sensor] = est;\n");
+ sb.append("\treturn est;\n");
+ sb.append("}\n\n");
+ if(usePWM) {
+ sb.append("int pwm_engine_fd = -1;\n");
+ sb.append("int pwm_steering_fd = -1;\n");
+ }
+ if(useDigits) {
+ sb.append("SegmentDisplay4x7 segment_display;\n");
+ sb.append("static void set_led_display (SegmentDisplay4x7 *segment_display, uint16_t value, bool showHex) {\n");
+ sb.append("static const uint8_t digits[] = {0x3f,0x06,0x5b,0x4f,\n");
+ sb.append(" 0x66,0x6d,0x7d,0x07,\n");
+ sb.append(" 0x7f,0x6f,0x77,0x7c,\n");
+ sb.append(" 0x39,0x5e,0x79,0x71};\n");
+ sb.append("if(showHex) {\n");
+ sb.append("uint8_t segments[4] = {digits[(value >> 12) & 0x0F], digits[(value >> 8) & 0x0F], digits[(value >> 4) & 0x0F], digits[value & 0x0F]};\n");
+ sb.append("segment_display_4x7_set_segments(segment_display, segments, 5, false);\n");
+ sb.append("} else {\n");
+ sb.append("uint8_t segments[4] = {digits[(value%10000)/1000], digits[(value%1000)/100], digits[(value%100)/10], digits[value%10]};\n");
+ sb.append("segment_display_4x7_set_segments(segment_display, segments, 5, false);\n");
+ sb.append("}\n");
+ sb.append("}\n\n");
+ }
+ if(useUS) {
+ sb.append("uint16_t ultra_sonic_A;\n");
+ sb.append("uint64_t us_A_last_cb_time = 0;\n");
+ sb.append("char* uid_us_A = \"zpW\";\n");
+ sb.append("void us_A_callback(uint16_t distance, void *data) {\n");
+ sb.append("ultra_sonic_A = kalman_filter(distance, 0);\n");
+ sb.append("us_A_last_cb_time = time_util_get_current_micros();\n");
+ sb.append("}\n");
+ sb.append("uint16_t ultra_sonic_B;\n");
+ sb.append("uint64_t us_B_last_cb_time = 0;\n");
+ sb.append("char* uid_us_B = \"zqN\";\n");
+ sb.append("void us_B_callback(uint16_t distance, void *data) {\n");
+ sb.append("us_B_last_cb_time = time_util_get_current_micros();\n");
+ sb.append("ultra_sonic_B = kalman_filter(distance, 1);\n");
+ sb.append("}\n\n");
+ }
+ if(useLaser) {
+ sb.append("int16_t laser_distance;\n");
+ sb.append("uint64_t laser_last_cb_time = 0;\n");
+ sb.append("void laser_callback(uint16_t distance, void *data) {\n");
+ sb.append("laser_distance = kalman_filter(distance, 2);\n");
+ sb.append("laser_last_cb_time = time_util_get_current_micros();\n");
+ sb.append("}\n\n");
+ }
+ if(useACC) {
+ sb.append("float acceleration_X, acceleration_Y, acceleration_Z;\n");
+ sb.append("uint64_t acc_last_cb_time = 0;\n");
+ sb.append("void acc_callback(int16_t x, int16_t y, int16_t z, void* data) {\n");
+ sb.append("if (abs(x - acceleration_X) > 2 * 10 * 100 ||\n");
+ sb.append("abs(y - acceleration_Y) > 2 * 10 * 100 ||\n");
+ sb.append("abs(z - acceleration_Z) > 2 * 10 * 100)\n");
+ sb.append(" return;\n");
+ sb.append("acceleration_X = kalman_filter(x, 3) / 100.0;\n");
+ sb.append("acceleration_Y = kalman_filter(y, 4) / 100.0;\n");
+ sb.append("acceleration_Z = kalman_filter(z, 5) / 100.0;\n");
+ sb.append("acc_last_cb_time = time_util_get_current_micros();\n");
+ sb.append("}\n\n");
+ sb.append("float angularVelocity_X, angularVelocity_Y, angularVelocity_Z;\n");
+ sb.append("uint64_t angV_last_cb_time = 0;\n");
+ sb.append("void angV_callback(int16_t x, int16_t y, int16_t z, void* data) {\n");
+ sb.append("angularVelocity_X = kalman_filter(x, 6) / 16;\n");
+ sb.append("angularVelocity_Y = kalman_filter(y, 7) / 16;\n");
+ sb.append("angularVelocity_Z = kalman_filter(z, 8) / 16;\n");
+ sb.append("angV_last_cb_time = time_util_get_current_micros();\n");
+ sb.append("}\n\n");
+ }
+ return sb.toString();
+ }
+
+ /** Returns the port and ID identifier. */
+ private String identifierName(INamedElement element) {
+ String name = element.getName() != null ? element.getName().replace(' ', '_') : "";
+ return name.replace(' ', '_') + "_ID_" + element.getId();
+ }
+
+ /** Creates the read code for the given receiver and port. */
+ private String createWriteCode(Transmitter transmitter, OutputPort outport) {
+ if(transmitter instanceof ActuatorPWM) {
+ ActuatorPWM act = (ActuatorPWM)transmitter;
+ String result = "if(!noval_" + identifierName(outport) + ") {\n";
+ if(act.getChannelID() == 0) {
+ result +=
+ "temp_actuator_device_set_target(pwm_engine_fd, 0, (uint16_t)" +
+ identifierName(outport) + ");\n";
+ } else {
+ result +=
+ "temp_actuator_device_set_target(pwm_steering_fd, 0, (uint16_t)" +
+ identifierName(outport) + ");\n";
+ }
+ result += "}\n";
+ return result;
+ }
+ if(transmitter instanceof ActuatorDigits) {
+ ActuatorDigits digits = (ActuatorDigits)transmitter;
+ String result = "if(!noval_" + identifierName(outport) + ") {\n";
+ result +=
+ "set_led_display(&segment_display, (uint16_t)" + identifierName(outport) +
+ ", " + digits.isShowHexValue() + ");\n";
+ result += "}\n";
+ return result;
+ }
+ if(transmitter instanceof ConsoleOutput) {
+ String result = "\tif(!noval_" + identifierName(outport) + ") {\n";
+ result +=
+ "\t\tprintf(\"" + outport.getName() + " = %i\\n\", " + identifierName(outport) +
+ ");\n";
+ result += "\t} else {\n";
+ result += "\t\tprintf(\"" + outport.getName() + " = NoVal\\n\");\n";
+ result += "\t}\n";
+ return result;
+ }
+ if(transmitter instanceof RumbleMagnitudeWeak) {
+ useRumble = true;
+ rumbleWeakPort = outport;
+ }
+ if(transmitter instanceof RumbleMagnitudeStrong) {
+ useRumble = true;
+ rumbleStrongPort = outport;
+ }
+ if(transmitter instanceof MotorControlOutput) {
+ String result = "if(!noval_" + identifierName(outport) + ") {\n";
+ result += "setVelocity(" + identifierName(outport) + ");\n";
+ result += "}\n";
+ return result;
+ }
+ return "";
+ }
+
+ /** Creates Code for Rumble */
+ private String createRumbleCode() {
+ StringBuilder sb = new StringBuilder();
+ sb.append("rumblepad_set_rumble(");
+ if(rumbleStrongPort != null) {
+ sb.append("!noval_" + identifierName(rumbleStrongPort) + " ? " +
+ identifierName(rumbleStrongPort) + " : 0, ");
+ } else {
+ sb.append("0, ");
+ }
+ if(rumbleWeakPort != null) {
+ sb.append("!noval_" + identifierName(rumbleWeakPort) + " ? " +
+ identifierName(rumbleWeakPort) + " : 0, ");
+ } else {
+ sb.append("0, ");
+ }
+ sb.append("cycle_time * 0.9, 0);\n");
+ return sb.toString();
+ }
+
+ /** Creates the read code for the given receiver and port. */
+ private String createReadCode(Receiver receiver, InputPort inport) {
+ if(receiver instanceof CameraDistanceLeft) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport)).append(" = camera_client_get_distance_left();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof CameraDistanceRight) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport)).append(" = camera_client_get_distance_right();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof CameraDetectionStateLeft) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport)).append(
+ " = camera_client_get_detection_state_left();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof CameraDetectionStateRight) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport)).append(
+ " = camera_client_get_detection_state_right();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof CameraYawAngle) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport)).append(" = camera_client_get_yaw_angle();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof UltraSonicSensor) {
+ StringBuilder sb = new StringBuilder();
+ UltraSonicSensor sensor = (UltraSonicSensor)receiver;
+ if(nrOfUsSensors == 0) {
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("if (!strcmp(\"" + sensor.getUniqueBrickletID() + "\", uid_us_A)) {\n");
+ sb.append("float diff_A = (curr_time - us_A_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_A < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = ultra_sonic_A;\n}\n");
+ sb.append("}\n");
+ } else if(nrOfUsSensors == 1) {
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("if (!strcmp(\"" + sensor.getUniqueBrickletID() + "\", uid_us_B)){\n");
+ sb.append("float diff_B = (curr_time - us_B_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_B < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = ultra_sonic_B;\n}\n");
+ sb.append("}\n");
+ }
+ nrOfUsSensors++;
+ return sb.toString();
+ }
+ if(receiver instanceof LaserRangeSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_laser = (curr_time - laser_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_laser < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = laser_distance;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AccelerationXSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_accX = (curr_time - acc_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_accX < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = acceleration_X;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AccelerationYSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_accY = (curr_time - acc_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_accY < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = acceleration_Y;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AccelerationZSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_accZ = (curr_time - acc_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_accZ < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = acceleration_Z;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AngularVelocityXSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_angV_X = (curr_time - angV_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_angV_X < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = angularVelocity_X;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AngularVelocityYSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_angV_Y = (curr_time - angV_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_angV_Y < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = angularVelocity_Y;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof AngularVelocityZSensor) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("float diff_angV_Z = (curr_time - angV_last_cb_time) / (1.0 * SECONDS_IN_MICROS);\n");
+ sb.append("if (diff_angV_Z < 1.0) {\n");
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = angularVelocity_Z;\n");
+ sb.append("} else {\n");
+ sb.append("noval_" + identifierName(inport) + " = true;\n");
+ sb.append("}\n");
+ return sb.toString();
+ }
+ if(receiver instanceof MotorControlInput) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("noval_" + identifierName(inport) + " = false;\n");
+ sb.append(identifierName(inport) + " = getVelocity();\n");
+ return sb.toString();
+ }
+ if(receiver instanceof Button1) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_1", inport);
+ }
+ if(receiver instanceof Button2) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_2", inport);
+ }
+ if(receiver instanceof Button3) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_3", inport);
+ }
+ if(receiver instanceof Button4) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_4", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.ButtonL1) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_L1", inport);
+ }
+ if(receiver instanceof ButtonL2) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_L2", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.ButtonR1) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_R1", inport);
+ }
+ if(receiver instanceof ButtonR2) {
+ return gamepadReadCode("gamepad_get_button_state", "GAMEPAD_BUTTON_R2", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.Right_StickX_Position) {
+ return gamepadReadCode("gamepad_get_axis_position",
+ "GAMEPAD_AXIS_RIGHT_STICK_HORIZONTAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.Right_StickY_Position) {
+ return gamepadReadCode("gamepad_get_axis_position",
+ "GAMEPAD_AXIS_RIGHT_STICK_VERTICAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.Left_StickX_Position) {
+ return gamepadReadCode("gamepad_get_axis_position",
+ "GAMEPAD_AXIS_LEFT_STICK_HORIZONTAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.gamepad.Left_StickY_Position) {
+ return gamepadReadCode("gamepad_get_axis_position", "GAMEPAD_AXIS_LEFT_STICK_VERTICAL",
+ inport);
+ }
+ if(receiver instanceof ButtonA) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_A", inport);
+ }
+ if(receiver instanceof ButtonB) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_B", inport);
+ }
+ if(receiver instanceof ButtonX) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_X", inport);
+ }
+ if(receiver instanceof ButtonY) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_Y", inport);
+ }
+ if(receiver instanceof ButtonStart) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_START", inport);
+ }
+ if(receiver instanceof ButtonSelect) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_SELECT", inport);
+ }
+ if(receiver instanceof ButtonHome) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_MODE", inport);
+ }
+ if(receiver instanceof DPadDown) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_DPAD_DOWN", inport);
+ }
+ if(receiver instanceof DPadUp) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_DPAD_UP", inport);
+ }
+ if(receiver instanceof DPadLeft) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_DPAD_LEFT", inport);
+ }
+ if(receiver instanceof DPadRight) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_DPAD_RIGHT", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonL1) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_L1", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonR1) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_R1", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonL3) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_L3", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.ButtonR3) {
+ return gamepadReadCode("rumblepad_get_button_state", "RUMBLEPAD_BUTTON_R3", inport);
+ }
+ if(receiver instanceof L2_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position", "RUMBLEPAD_AXIS_L2", inport);
+ }
+ if(receiver instanceof R2_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position", "RUMBLEPAD_AXIS_R2", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.Left_StickX_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position",
+ "RUMBLEPAD_AXIS_LEFT_STICK_HORIZONTAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.Left_StickY_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position",
+ "RUMBLEPAD_AXIS_LEFT_STICK_VERTICAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.Right_StickX_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position",
+ "RUMBLEPAD_AXIS_RIGHT_STICK_HORIZONTAL", inport);
+ }
+ if(receiver instanceof org.fortiss.af3.platform.raspberry.model.rumblepad.Right_StickY_Position) {
+ return gamepadReadCode("rumblepad_get_axis_position",
+ "RUMBLEPAD_AXIS_RIGHT_STICK_VERTICAL", inport);
+ }
+ return "";
+ }
+
+ /** Creates read code for gamepad state. */
+ private String gamepadReadCode(String gamepadFun, String btnId, InputPort inport) {
+ String noval = "\tnoval_" + identifierName(inport) + " = false;\n";
+ String setBtn = "\t" + identifierName(inport) + " = " + gamepadFun + "(" + btnId + ");\n";
+ return noval + setBtn;
+ }
+
+ /** Create the initialize code. */
+ private String createInitCode(List<Pair<ExecutionUnit, Component>> deployedComponents) {
+ int createdUsSensors = 0;
+ StringBuilder sb = new StringBuilder();
+ if(useCamera) {
+ String addr = executionUnit.getCameraServerAddress();
+ String port = executionUnit.getCameraServerPort();
+ sb.append("camera_client_initialize(\"" + addr + "\", \"" + port + "\");\n");
+ }
+ if(useUS || useLaser || useDigits || useACC) {
+ sb.append("IPConnection brick_connection;\n");
+ sb.append("ipcon_create(&brick_connection);\n");
+ sb.append("if(ipcon_connect(&brick_connection, BRICK_HOST, BRICK_PORT) < 0) {\n");
+ sb.append("perror(\"Failed to connect to brick sub-system.\");\n");
+ sb.append("return 1;\n");
+ sb.append("}\n\n");
+ }
+ if(useUS) {
+ for(Pair<PlatformConnectorUnit, Port> p : deployedPorts) {
+ Receiver rec = atomicInputToReceiver.get(p.getSecond());
+ if(createdUsSensors >= 2)
+ break;
+ if(rec instanceof UltraSonicSensor) {
+ UltraSonicSensor sensor = (UltraSonicSensor)p.getFirst();
+ if(createdUsSensors == 0) {
+ sb.append("DistanceUS DistanceUS_A;\n");
+ sb.append("uid_us_A = \"" + sensor.getUniqueBrickletID() + "\";\n");
+ sb.append("distance_us_create(&DistanceUS_A, \"" +
+ sensor.getUniqueBrickletID() + "\", &brick_connection);\n");
+ sb.append("distance_us_register_callback(&DistanceUS_A, DISTANCE_US_CALLBACK_DISTANCE, (void*)us_A_callback, NULL);\n");
+ sb.append("distance_us_set_distance_callback_period(&DistanceUS_A, 10);\n");
+ } else {
+ sb.append("DistanceUS DistanceUS_B;\n");
+ sb.append("uid_us_B = \"" + sensor.getUniqueBrickletID() + "\";\n");
+ sb.append("distance_us_create(&DistanceUS_B, \"" +
+ sensor.getUniqueBrickletID() + "\", &brick_connection);\n");
+ sb.append("distance_us_register_callback(&DistanceUS_B, DISTANCE_US_CALLBACK_DISTANCE, (void*)us_B_callback, NULL);\n");
+ sb.append("distance_us_set_distance_callback_period(&DistanceUS_B, 10);\n");
+ }
+ createdUsSensors++;
+ }
+ }
+ }
+ if(useLaser) {
+ sb.append("LaserRangeFinder laser;\n");
+ sb.append("laser_range_finder_create(&laser, \"CQ9\", &brick_connection);\n");
+ sb.append("laser_range_finder_enable_laser(&laser);\n");
+ sb.append("laser_range_finder_register_callback(&laser, LASER_RANGE_FINDER_CALLBACK_DISTANCE, (void*)laser_callback, NULL);\n");
+ sb.append("laser_range_finder_set_distance_callback_period(&laser, 10);\n");
+ }
+ if(useDigits) {
+ sb.append("segment_display_4x7_create(&segment_display, \"wNK\", &brick_connection);\n");
+ }
+ if(useACC) {
+ sb.append("IMUV2 imu;\n");
+ sb.append("imu_v2_create(&imu, \"6xCD6s\", &brick_connection);\n");
+ sb.append("imu_v2_register_callback(&imu, IMU_V2_CALLBACK_LINEAR_ACCELERATION, (void*)acc_callback, NULL);\n");
+ sb.append("imu_v2_set_linear_acceleration_period(&imu, 5);\n");
+ sb.append("imu_v2_register_callback(&imu, IMU_V2_CALLBACK_ANGULAR_VELOCITY, (void*)angV_callback, NULL);\n");
+ sb.append("imu_v2_set_angular_velocity_period(&imu, 5);\n");
+ }
+ if(usePWM) {
+ sb.append("pwm_steering_fd = temp_actuator_initialize(\"/dev/ttyACM0\");\n");
+ sb.append("pwm_engine_fd = temp_actuator_initialize(\"/dev/ttyACM2\");\n\n");
+ }
+ if(useGamepad) {
+ sb.append("gamepad_configuration_t* gamepad_config = malloc(sizeof(gamepad_configuration_t));\n");
+ sb.append("gamepad_config->device_id = \"/dev/input/js0\";\n");
+ sb.append("gamepad_config->waiting_sleep_in_micros = " + WAITING_SLEEP_IN_MICROS +
+ ";\n");
+ sb.append("gamepad_config->axis_callback = NULL;\n");
+ sb.append("gamepad_config->button_callback = NULL;\n");
+ sb.append("gamepad_initialize(gamepad_config);\n\n");
+ }
+ if(useRumblepad) {
+ sb.append("while(access(\"/dev/input/js0\", F_OK) == -1) {\n");
+ sb.append("sleep(1);\n");
+ sb.append("printf(\"Could not access gamepad device. Trying again in 1s.\\n\");\n");
+ sb.append("}\n");
+ sb.append("rumblepad_configuration_t* rumblepad_config = malloc(sizeof(rumblepad_configuration_t));\n");
+ sb.append("rumblepad_config->device_id = \"/dev/input/js0\";\n");
+ sb.append("rumblepad_config->waiting_sleep_in_micros = " + WAITING_SLEEP_IN_MICROS +
+ ";\n");
+ sb.append("rumblepad_config->axis_callback = NULL;\n");
+ sb.append("rumblepad_config->button_callback = NULL;\n");
+ sb.append("rumblepad_initialize(rumblepad_config);\n\n");
+ }
+ if(useVesc) {
+ sb.append("initUSB(\"/dev/vesc\", B115200);\n");
+ }
+ return sb.toString();
+ }
+
+ /** Creates the function call code. */
+ private String makeCall(String function, Component c) {
+ StringBuilder sb = new StringBuilder();
+ sb.append(function).append('_').append(c.getName());
+ sb.append("_ID_").append(c.getId()).append("();\n");
+ return sb.toString();
+ }
+
+ /** Find sub-structure of component port targets. */
+ public static List<Port> findTargetPorts(Component boundary, Port port) {
+ List<Port> targets = new ArrayList<Port>();
+ if(isAtomicComponent(port.getComponent())) {
+ targets.add(port);
+ return targets;
+ }
+ LinkedList<Port> explore = new LinkedList<Port>();
+ for(Channel ch : port.getOutgoingChannels()) {
+ explore.add(ch.getTarget());
+ }
+ while(!explore.isEmpty()) {
+ Port testee = explore.removeFirst();
+ if(testee.getComponent() == boundary || isAtomicComponent(testee.getComponent()) ||
+ testee.getOutgoing().isEmpty()) {
+ targets.add(testee);
+ } else {
+ for(Channel ch : testee.getOutgoingChannels()) {
+ explore.add(ch.getTarget());
+ }
+ }
+ }
+ return targets;
+ }
+}
diff --git a/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/templates/MultiUnitMainFile.stg b/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/templates/MultiUnitMainFile.stg
new file mode 100644
index 0000000000000000000000000000000000000000..d37961543e8d792adf624ad75570b45906943f75
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry/src/org/fortiss/af3/platform/raspberry/generator/templates/MultiUnitMainFile.stg
@@ -0,0 +1,117 @@
+group MainFile;
+
+MainFile(UNIT_NAME,
+ COORDINATOR_OR_WORKER,
+ CAN_ID_LOCAL_UNIT,
+ SYSTEM_INIT_CODE,
+ SENSOR_VARIABLES,
+ REMOTE_UNIT_SETUP_CODE,
+ SYSTEM_INCLUDES,
+ COMPONENT_PORTS,
+ COMPONENT_SETUP,
+ COMPONENT_IMPL,
+ CYCLE_TIME_IN_MILLIS,
+ THREAD_SLEEPTIME_IN_MICROS) ::= <<
+// due to current data dictionary declaration of GENTYPE_boolean
+// system include must be first
+/******************** BEGIN_@GENERATED ***********************/
+$SYSTEM_INCLUDES$
+/********************* END_@GENERATED ************************/
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <unistd.h>
+
+#include <af3.h>
+#include <af3_component.h>
+#include <af3_component_remote_proxy.h>
+#include <debugprint.h>
+#include <listutil.h>
+#include <protocol_control_center.h>
+#include <protocol_coordinator.h>
+#include <protocol_factory.h>
+#include <timeutil.h>
+
+#include "data.h"
+
+int global_debug_print_level = DEBUG_PRINT_LEVEL_NONE;
+const int cycle_time = $CYCLE_TIME_IN_MILLIS$;
+#define BRICK_HOST "localhost"
+#define BRICK_PORT 4223
+
+/******************** BEGIN_@GENERATED ***********************/
+$COMPONENT_PORTS$
+
+$SENSOR_VARIABLES$
+/********************* END_@GENERATED ************************/
+
+/******************** BEGIN_@GENERATED ***********************/
+$COMPONENT_IMPL$
+/********************* END_@GENERATED ************************/
+
+static void initialize_af3_subsystem() {
+ af3_module_initialize("$UNIT_NAME$", 0, NULL, NULL);
+ af3_component_module_initialize($THREAD_SLEEPTIME_IN_MICROS$, default_send_remote_impl);
+ af3_component_remote_proxy_module_initialize();
+ can_thread_add_reception_listener(default_recv_remote_impl);
+
+/******************** BEGIN_@GENERATED ***********************/
+$COMPONENT_SETUP$
+/********************* END_@GENERATED ************************/
+}
+
+static void worker() {
+ af3_component_run_system();
+ af3_component_remote_proxy_clear_inputs();
+}
+
+int main(int argc, char** argv) {
+ if(argc >= 2) {
+ int dbg = atoi(argv[1]);
+ if(dbg < 0) {
+ dbg = 0;
+ } else if (dbg > 3) {
+ dbg = 3;
+ }
+ global_debug_print_level = dbg;
+ }
+ if (!can_thread_create("can0")) {
+ return -1;
+ }
+ can_thread_set_thread_sleep_in_micros($THREAD_SLEEPTIME_IN_MICROS$);
+
+/******************** BEGIN_@GENERATED ***********************/
+ $SYSTEM_INIT_CODE$
+/********************* END_@GENERATED ************************/
+
+ initialize_af3_subsystem();
+
+ protocol_can_config_t* can_config = protocol_can_config_create(0x01, 0x00, $CAN_ID_LOCAL_UNIT$);
+
+ protocol_worker_clock_config_t* clock_config =
+ protocol_worker_clock_config_create(cycle_time, 0, $THREAD_SLEEPTIME_IN_MICROS$);
+
+ protocol_worker_computation_config_t* computation_config =
+ protocol_worker_computation_config_create("$UNIT_NAME$", worker, 0);
+
+#if $COORDINATOR_OR_WORKER$ == 1
+ // create list of remote units
+ list_t* remote_units = list_util_create_list();
+/******************** BEGIN_@GENERATED ***********************/
+ $REMOTE_UNIT_SETUP_CODE$
+/********************* END_@GENERATED ************************/
+ // run the unit coordinator protocol mode
+ int result = protocol_coordinator_main(can_config, clock_config, computation_config, remote_units);
+#else
+ // run the unit in worker protocol mode
+ int result = protocol_worker_main(can_config, clock_config, computation_config);
+#endif
+ can_thread_terminate();
+
+ free(can_config);
+ free(clock_config);
+ free(computation_config);
+
+ return result;
+}
+>>