Commit 86517ff7 authored by Sven Robertz's avatar Sven Robertz
Browse files

added Oscar's RAPID generation and lib

parent 8a0eeb9d
......@@ -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);
}
}
}
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());
}
}
<?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
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
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
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();
}
}
}
}
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")]
<?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">