added Oscar's RAPID generation and lib

compiler/LabComm.java

@@ -80,6 +80,7 @@ public class LabComm {
     String pythonFile = null;
     String prettyFile = null;
     String typeinfoFile = null;
+	String rapidFile = null;
 
     for (int i = 0 ; i < args.length ; i++) {
       if (fileName == null ||
@@ -119,6 +120,8 @@ public class LabComm {
 	prettyFile = args[i].substring(9);
       } else if (args[i].startsWith("--typeinfo=")) {
 	typeinfoFile = args[i].substring(11);
+      } else if (args[i].equals("--rapid")) {
+	rapidFile = coreName + ".sys";
       } else if (i == args.length - 1) {
 	fileName = args[i];
       } else {
@@ -192,6 +195,13 @@ public class LabComm {
 	  genPython(ast, pythonFile, prefix);
 	  prettyOnStdout = false;
 	}
+	if (rapidFile != null) {
+		if (verbose) {
+			System.err.println("Generating RAPID: " + rapidFile);
+		}
+		genRAPID(ast, rapidFile, coreName);
+		prettyOnStdout = false;
+	}
 	if (prettyFile != null) {
 	  if (verbose) { 
 	    System.err.println("Generating Pretty: " + prettyFile); 
@@ -290,6 +300,13 @@ public class LabComm {
     }
   }
 
+  private static void genRAPID(Program p, String filename, String prefix) {
+    try {
+      p.RAPID_gen(filename, prefix);
+    } catch (IOException e) {
+      System.err.println("IOException: " + filename + " " + e);
+    }
+  }
 
 
 }

compiler/RAPID_CodeGen.jrag

+
+aspect RAPID_env {
+	public class RAPID_env {
+		private String prefix;
+		private StringBuilder types;
+		private StringBuilder constants;
+		private StringBuilder procedures;
+		private PrintStream ps;
+
+		public RAPID_env(PrintStream ps, String prefix)
+		{
+			this.types = new StringBuilder();
+			this.constants = new StringBuilder();
+			this.procedures = new StringBuilder();
+			this.prefix = prefix;
+			this.ps = ps;
+		}
+
+		public String prefix() { return this.prefix; }
+
+		public String addRecord(String name, java.util.List<String> components)
+		{
+			String recordName = this.prefix + "_" + name;
+			types.append("\tRECORD " + recordName);
+			types.append("\n");
+			for (String c : components) {
+				types.append("\t\t" + c + "\n");
+			}
+			types.append("\tENDRECORD");
+			types.append("\n\n");
+			return recordName;
+		}
+
+		public void addConstant(String type, String name, String value) {
+			this.constants.append("\tLOCAL CONST " + type + " " + name +
+					" := " + value + ";\n");
+		}
+
+		public void addProc(String name, java.util.List<String> params,
+					java.util.List<String> stmts)
+		{
+			this.procedures.append("\tLOCAL PROC " + name + "(");
+			for (int i = 0; i < params.size(); i++) {
+				this.procedures.append(params.get(i));
+				if (i < params.size() - 1) {
+					this.procedures.append(", ");
+				}
+			}
+			this.procedures.append(")\n");
+			for (String stmt : stmts) {
+				this.procedures.append("\t\t" + stmt + "\n");
+			}
+			this.procedures.append("\tERROR\n\t\tRAISE ;\n\tENDPROC\n\n");
+		}
+
+		public void flush()
+		{
+			ps.println("MODULE " + prefix() + "(SYSMODULE)");
+			ps.println();
+			ps.print(types.toString());
+			ps.println();
+			ps.println("\tLOCAL CONST string prefix:=\"" + this.prefix + "\";");
+			ps.print(constants.toString());
+			ps.println();
+			ps.print(procedures.toString());
+			ps.println();
+			ps.print("ENDMODULE");
+		}
+	}
+}
+
+aspect RAPID_CodeGen {
+
+	public void ASTNode.RAPID_gen(RAPID_env env) {
+		throw new UnsupportedOperationException();
+	}
+
+	public void Program.RAPID_gen(String file, String prefix)
+			throws IOException
+	{
+		PrintStream ps = new PrintStream(new FileOutputStream(new File(file)));
+		RAPID_env env = new RAPID_env(ps, prefix);
+		RAPID_gen(env);
+	}
+
+	public void Program.RAPID_gen(RAPID_env env)
+	{
+		for (int i = 0; i < getNumDecl(); i++) {
+			getDecl(i).RAPID_gen(env);
+		}
+		env.flush();
+	}
+
+	public void Decl.RAPID_gen(RAPID_env env) {
+		throw new UnsupportedOperationException();
+	}
+
+	public void SampleDecl.RAPID_gen(RAPID_env env) {
+		// Add type declarations
+		String fullName = getType().RAPID_AddType(env, getName());
+		// Add signature constants
+		String sig_len_name = "signature_len_" + getName();
+		String sig_name = "signature_" + getName();
+		SignatureList sig = signature();
+		StringBuilder sb = new StringBuilder();
+		sb.append("[");
+		byte[] d = null;
+		int sig_len = 0;
+		for (int i = 0; i < sig.size(); i++) {
+			d = sig.getData(i);
+			for (int j = 0; d != null && j < d.length; j++) {
+				sb.append(d[j] + ",");
+				sig_len++;
+			}
+		}
+		sb.delete(sb.length() - 1, sb.length());
+		sb.append("]");
+		env.addConstant("num", sig_len_name, "" + sig_len);
+		env.addConstant("byte", sig_name + "{" + sig_len_name + "}",
+			sb.toString());
+		
+		// Add decode procedures
+		ArrayList<String> params = new ArrayList<String>();
+		ArrayList<String> stmts = new ArrayList<String>();
+		params.add("VAR LabComm_Decoder_Sample s");
+		params.add("string handler");
+		stmts.add("s.prefix := prefix;");
+		stmts.add("s.name := \"" + getName() + "\";");
+		stmts.add("s.handler := handler;");
+		env.addProc("Dec_Reg_" + getName(), params, stmts);	
+
+		params.clear();
+		stmts.clear();
+		params.add("VAR LabComm_Decoder_Sample s");
+		params.add("VAR rawbytes sig");
+		params.add("num user_id");
+		stmts.add("VAR byte tmp_sig{" + sig_len_name + "};");
+		stmts.add("IF RawBytesLen(sig)<>" + sig_len_name + " THEN");
+		stmts.add("\tRETURN;");
+		stmts.add("ENDIF");
+		stmts.add("FOR i FROM 1 TO " + sig_len_name + " DO");
+		stmts.add("\tUnpackRawBytes sig, i, tmp_sig{i}, \\Hex1;");
+		stmts.add("ENDFOR");
+		stmts.add("IF tmp_sig<>" + sig_name + " THEN");
+		stmts.add("\tRETURN;");
+		stmts.add("ENDIF");
+		stmts.add("s.user_id := user_id;");
+		env.addProc("Reg_If_Signature_Of_" + getName(), params, stmts);
+
+		params.clear();
+		stmts.clear();
+		params.add("VAR Decoder d");
+		params.add("VAR LabComm_Stream st");
+		params.add("VAR LabComm_Decoder_Sample s");
+		stmts.add("VAR " + fullName + " tmp;");
+		getType().RAPID_AddDecodeInstr(env, stmts, "tmp", "st");
+		stmts.add("% s.handler % tmp;");
+		env.addProc("Decode_And_Handle_" + getName(), params, stmts);
+
+		params.clear();
+		stmts.clear();
+		params.add("VAR Encoder e");
+		params.add("VAR LabComm_Stream st");
+		params.add("VAR LabComm_Encoder_Sample s");
+		stmts.add("s.prefix := prefix;");
+		stmts.add("s.name := \"" + getName() + "\";");
+		stmts.add("Encoder_Register_Sample e, st, s;");
+		env.addProc("Enc_Reg_" + getName(), params, stmts);
+
+		params.clear();
+		stmts.clear();
+		params.add("VAR Encoder e");
+		params.add("VAR LabComm_Stream s");
+		stmts.add("VAR rawbytes buffer;");
+		stmts.add("FOR i FROM 1 TO " + sig_len_name + " DO");
+		stmts.add("\tPackRawBytes " + sig_name +
+					"{i}, buffer, \\Network, i, \\Hex1;");
+		stmts.add("ENDFOR");
+		stmts.add("SocketSend s.soc, \\RawData:=buffer, \\NoOfBytes:=" +
+					sig_len_name + ";");
+		env.addProc("Encode_Signature_" + getName(), params, stmts);
+
+		params.clear();
+		stmts.clear();
+		params.add("VAR Encoder e");
+		params.add("VAR LabComm_Stream st");
+		params.add("VAR LabComm_Encoder_Sample s");
+		params.add("VAR " + fullName + " val");
+		stmts.add("Encode_Packed st, s.user_id;");
+		getType().RAPID_AddEncodeInstr(env, stmts, "val", "st");
+		env.addProc("Encode_" + getName(), params, stmts);
+	}
+
+	public String Type.RAPID_AddType(RAPID_env env, String name) {
+		throw new UnsupportedOperationException();
+	}
+
+	public String StructType.RAPID_AddType(RAPID_env env, String name) {
+		ArrayList<String> components = new ArrayList<String>();
+		for (int i = 0; i < getNumField(); i++) {
+			Field f = getField(i);
+			components.add(
+					f.getType().RAPID_AddType(env, name + "_" + f.getName()) +
+					" " + f.getName() + ";");
+		}
+		String typeName = env.addRecord(name, components);
+		return typeName;
+	}
+
+	public String FixedArrayType.RAPID_AddType(RAPID_env env, String name) {
+		String typeName = getType().RAPID_AddType(env, name + "_e");
+		if (getNumExp() > 1) {
+			throw new UnsupportedOperationException();
+		}
+		ArrayList<String> components = new ArrayList<String>();
+		for (int i = 1; i <= getExp(0).RAPID_getValue(); i++) {
+			components.add(typeName + " e" + i + ";");
+		}
+		String completeName = env.addRecord("list_" + name, components);
+		return completeName;
+	}
+
+	public String PrimType.RAPID_AddType(RAPID_env env, String name) {
+		if (getToken() == LABCOMM_SHORT ||
+			getToken() == LABCOMM_FLOAT ||
+			getToken() == LABCOMM_INT) {
+			return "num";
+		} else if (getToken() == LABCOMM_LONG) {
+			return "dnum";
+		} else if (getToken() == LABCOMM_STRING) {
+			return "string";
+		} else if (getToken() == LABCOMM_BOOLEAN) {
+			return "bool";
+		} else if (getToken() == LABCOMM_BYTE) {
+			return "byte";
+		}
+		throw new UnsupportedOperationException();
+	}
+
+	public void Type.RAPID_AddDecodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		throw new UnsupportedOperationException();
+	}
+
+	public void StructType.RAPID_AddDecodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		for (int i = 0; i < getNumField(); i++) {
+			getField(i).getType().RAPID_AddDecodeInstr(env, instrs,
+					var_name + "." + getField(i).getName(), stream_name);
+		}
+	}
+
+	public void FixedArrayType.RAPID_AddDecodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		for (int i = 1; i <= getExp(0).RAPID_getValue(); i++) {
+			getType().RAPID_AddDecodeInstr(env, instrs,
+					var_name + ".e" + i, stream_name);
+		}
+	}
+
+	public void PrimType.RAPID_AddDecodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		switch(getToken()) {
+			case LABCOMM_BYTE:
+				instrs.add("Decode_Byte " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_BOOLEAN:
+				instrs.add("Decode_Bool " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_SHORT:
+				instrs.add("Decode_Short " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_INT:
+				instrs.add("Decode_Int " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_LONG:
+				instrs.add("Decode_Long " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_FLOAT:
+				instrs.add("Decode_Float " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_STRING:
+				instrs.add("Decode_String " + stream_name + "," + var_name + ";");
+				break;
+			default:
+				throw new UnsupportedOperationException();
+		}
+	}
+
+	public void Type.RAPID_AddEncodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		throw new UnsupportedOperationException();
+	}
+
+	public void StructType.RAPID_AddEncodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		for (int i = 0; i < getNumField(); i++) {
+			getField(i).getType().RAPID_AddEncodeInstr(env, instrs,
+					var_name + "." + getField(i).getName(), stream_name);
+		}
+	}
+
+	public void FixedArrayType.RAPID_AddEncodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		for (int i = 1; i <= getExp(0).RAPID_getValue(); i++) {
+			getType().RAPID_AddEncodeInstr(env, instrs,
+					var_name + ".e" + i, stream_name);
+		}
+	}
+
+	public void PrimType.RAPID_AddEncodeInstr(RAPID_env env,
+			java.util.List<String> instrs,
+			String var_name, String stream_name) {
+		switch(getToken()) {
+			case LABCOMM_BYTE:
+				instrs.add("Encode_Byte " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_BOOLEAN:
+				instrs.add("Encode_Bool " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_SHORT:
+				instrs.add("Encode_Short " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_INT:
+				instrs.add("Encode_Int " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_LONG:
+				instrs.add("Encode_Long " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_FLOAT:
+				instrs.add("Encode_Float " + stream_name + "," + var_name + ";");
+				break;
+			case LABCOMM_STRING:
+				instrs.add("Encode_String " + stream_name + "," + var_name + ";");
+				break;
+			default:
+				throw new UnsupportedOperationException();
+		}
+	}
+
+	public int Exp.RAPID_getValue() {
+		throw new UnsupportedOperationException();
+	}
+
+	public int IntegerLiteral.RAPID_getValue() {
+		return Integer.parseInt(getValue());
+	}
+}

examples/robot/App.config

+<?xml version="1.0" encoding="utf-8" ?>
+<configuration>
+    <startup> 
+        <supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5" />
+    </startup>
+</configuration>
\ No newline at end of file

examples/robot/Follow.mod

+MODULE Follow
+    PROC LCjt2jt(VAR LCRobot_jointtarget a, VAR jointtarget b)
+        b.robax.rax_1 := a.robax.rax_1;
+        b.robax.rax_2 := a.robax.rax_2;
+        b.robax.rax_3 := a.robax.rax_3;
+        b.robax.rax_4 := a.robax.rax_4;
+        b.robax.rax_5 := a.robax.rax_5;
+        b.robax.rax_6 := a.robax.rax_6;
+        
+        b.extax.eax_a := a.extax.eax_a;
+        b.extax.eax_b := a.extax.eax_b;
+        b.extax.eax_c := a.extax.eax_c;
+        b.extax.eax_d := a.extax.eax_d;
+        b.extax.eax_e := a.extax.eax_e;
+        b.extax.eax_f := a.extax.eax_f;
+    ENDPROC
+    
+    PROC handle_jointtarget(LCRobot_jointtarget val)
+        VAR jointtarget jt;
+        LCjt2jt val, jt;
+        MoveAbsJ jt, v100, z1, tool0, \WObj:=wobj0;
+    ENDPROC
+    
+    PROC main()
+        VAR LabComm_Stream st;
+        VAR socketdev srv;
+        SocketCreate srv;
+        SocketBind srv, "127.0.0.1", 51337;
+        SocketListen srv;
+        WHILE TRUE DO
+            SocketAccept srv, st.soc;
+            Receive_targets st;
+        ENDWHILE
+    ENDPROC
+    
+    PROC Receive_targets(VAR LabComm_Stream st)
+        VAR Decoder d;
+        VAR LabComm_Decoder_Sample s{1};
+        Init_Decoder d, st;
+        % "LCRobot:Dec_Reg_jointtarget" % s{1}, "handle_jointtarget";
+        WHILE TRUE DO
+            Decode_One d, st, s;
+        ENDWHILE
+    ERROR
+        SocketClose st.soc;
+        RETURN;
+    ENDPROC
+    
+ENDMODULE
\ No newline at end of file

examples/robot/LCRobot.lc

+sample struct {
+	struct {
+		float x;
+		float y;
+		float z;
+	} trans;
+	struct {
+		float q1;
+		float q2;
+		float q3;
+		float q4;
+	} rot;
+	struct {
+		short cf1;
+		short cf4;
+		short cf6;
+		short cfx;
+	} robconfig;
+} robtarget;
+
+sample struct {
+	struct {
+		float rax_1;
+		float rax_2;
+		float rax_3;
+		float rax_4;
+		float rax_5;
+		float rax_6;
+	} robax;
+	struct {
+		float eax_a;
+		float eax_b;
+		float eax_C;
+		float eax_d;
+		float eax_e;
+		float eax_f;
+	} extax;
+} jointtarget;
\ No newline at end of file

examples/robot/Program.cs

+using se.lth.control.labcomm;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace RobotCtrl
+{
+    class Program
+    {
+
+        public static string IP_ADDRESS = "127.0.0.1";
+        public static int PORT = 51337;
+
+        static void Main(string[] args)
+        {
+            jointtarget val = new jointtarget { robax = new jointtarget.struct_robax(), extax = new jointtarget.struct_extax() };
+            TcpClient client = new TcpClient();
+            IPEndPoint serverEndPoint = new IPEndPoint(IPAddress.Parse(Program.IP_ADDRESS), Program.PORT);
+            try
+            {
+                client.Connect(serverEndPoint);
+                LabCommEncoder enc = new LabCommEncoderChannel(client.GetStream(), true);
+                jointtarget.register(enc);
+                jointtarget.encode(enc, val);
+                for (int i = 0; i < 10; i++)
+                {
+                    val.robax.rax_2 += 2;
+                    jointtarget.encode(enc, val);
+                }
+                for (int i = 0; i < 20; i++)
+                {
+                    val.robax.rax_2 -= 2;
+                    jointtarget.encode(enc, val);
+                }
+                for (int i = 0; i < 10; i++)
+                {
+                    val.robax.rax_2 += 2;
+                    jointtarget.encode(enc, val);
+                }
+            }
+            finally
+            {
+                client.Close();
+            }
+        }
+    }
+}

examples/robot/Properties/AssemblyInfo.cs

+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// General Information about an assembly is controlled through the following 
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("RobotCtrl")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("RobotCtrl")]
+[assembly: AssemblyCopyright("Copyright ©  2013")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible 
+// to COM components.  If you need to access a type in this assembly from 
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("bd5ee6d6-1dff-4043-8ca9-b56a29116e61")]
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version 
+//      Build Number
+//      Revision
+//
+// You can specify all the values or you can default the Build and Revision Numbers 
+// by using the '*' as shown below:
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]

examples/robot/RobotCtrl.csproj

+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{F0051A25-4FFF-4421-A5CB-01B512D6CEC5}</ProjectGuid>
+    <OutputType>Exe</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>RobotCtrl</RootNamespace>
+    <AssemblyName>RobotCtrl</AssemblyName>
+    <TargetFrameworkVersion>v4.5</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Data.DataSetExtensions" />
+    <Reference Include="Microsoft.CSharp" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Xml" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="LCRobot.cs" />
+    <Compile Include="Program.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="App.config" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\lib\csharp\LabComm.csproj">
+      <Project>{755cd5a6-c48e-4d35-b0be-8ec0fde1a2a1}</Project>
+