public bool PointRotor(IMyTerminalBlock rotor, double ang, double speed = 10)
{
double targAng = ang * 57.2958;
if (!rotors.ContainsKey(rotor))
{
rotors.Add(rotor, new ROTORPID(targAng, -speed, speed, false));
}
rotors[rotor].SetPoint = targAng;
double currentAngle = (rotor as IMyMotorStator).Angle * 57.2958;
if (currentAngle > 180) currentAngle -= 360;
double deltaAng = targAng - currentAngle;
if (deltaAng > 180) deltaAng -= 360;
if (deltaAng < -180) deltaAng += 360;
double rpm = rotors[rotor].Compute(currentAngle);
if (rotors[rotor].lastError > -0.1 && rotors[rotor].lastError < 0.1)
{
rotor.SetValueFloat("Velocity", 0);
rotors.Remove(rotor);
return true;
}
rotor.SetValueFloat("Velocity", (float)rpm);
return false;
}
public bool PointRotor(IMyTerminalBlock rotor, Vector3D targ, IMyTerminalBlock reference)
{
double targAng = GetTargetRotorAngle(rotor, targ, reference) * 57.2958;
if (!rotors.ContainsKey(rotor))
{
rotors.Add(rotor, new ROTORPID(targAng, -10, 10, false));
}
rotors[rotor].SetPoint = targAng;
double currentAngle = (rotor as IMyMotorStator).Angle * 57.2958;
if (currentAngle > 180) currentAngle -= 360;
double rpm = rotors[rotor].Compute(currentAngle);
rotor.SetValueFloat("Velocity", (float)rpm);
return false;
}
public void ExecuteInstruction()
{
if (Instructions.Count <= 0 || Instructions.Count < Line)
{
return;
}
Instruction instruction = Instructions[Line];
string name, mode, action, search, property, source;
float value1 = 0f;
float value2 = 0f;
float epsilon = 0.1f;
bool bool1 = false;
bool bool2 = false;
Object var;
switch (instruction.Command)
{
case Command.Comment:
Line++;
break;
case Command.Var:
// var <var name> <float value>
name = instruction.Fields[0];
ParseFieldFloat(instruction.Fields[1], out value1);
Vars.Add(name, value1);
Line++;
break;
case Command.Add:
case Command.Sub:
case Command.Mul:
case Command.Div:
// <operator> <var name> <var name|float value> <float value>
name = instruction.Fields[0];
ParseFieldFloat(instruction.Fields[1], out value1);
ParseFieldFloat(instruction.Fields[2], out value2);
float result = 0f;
switch (instruction.Command)
{
case Command.Add:
result = value1 + value2;
break;
case Command.Sub:
result = value1 - value2;
break;
case Command.Mul:
result = value1 * value2;
break;
case Command.Div:
result = value1 / value2;
break;
}
Vars[name] = result;
Line++;
break;
case Command.Print:
bool append = true;
string message = ParseFieldString(instruction.Fields[0]);
message = "\n" + message;
if (instruction.Fields.Count > 1)
{
List <string> args = new List <string>();
for (int i = 1; i < instruction.Fields.Count; i++)
{
float value;
ParseFieldFloat(instruction.Fields[1], out value);
args.Add(value.ToString());
}
myProgram.drawingSurface.WriteText(string.Format(message, args.ToArray()), append);
}
else
{
myProgram.drawingSurface.WriteText(message, append);
}
Line++;
break;
case Command.Select:
// select <var name> <mode> <string value>
//myProgram.drawingSurface.WriteText($"\nSelect fields={instruction.Fields.Count}", true);
name = instruction.Fields[0];
mode = instruction.Fields[1];
search = instruction.Fields[2];
List <IMyTerminalBlock> blocks = new List <IMyTerminalBlock>();
switch (mode)
{
case "tag":
myProgram.GridTerminalSystem.GetBlocksOfType <IMyTerminalBlock>(blocks, myBlock => myBlock.CustomName.Contains(search));
if (blocks.Count > 0)
{
Vars.Add(name, blocks);
}
break;
case "group":
IMyBlockGroup group = myProgram.GridTerminalSystem.GetBlockGroupWithName(search);
group.GetBlocks(blocks);
if (blocks.Count > 0)
{
Vars.Add(name, blocks);
}
break;
default:
IMyTerminalBlock block = myProgram.GridTerminalSystem.GetBlockWithName(search);
if (block != null)
{
blocks.Add(block);
Vars.Add(name, blocks);
}
break;
}
Line++;
break;
case Command.Action:
// action <var name> <action>
name = instruction.Fields[0];
action = instruction.Fields[1];
Vars.TryGetValue(name, out var);
if (instruction.Fields.Count == 3)
{
ParseFieldFloat(instruction.Fields[2], out value1);
}
if (var != null)
{
blocks = (List <IMyTerminalBlock>)var;
blocks.ForEach(delegate(IMyTerminalBlock block) {
block.ApplyAction(action);
});
}
Line++;
break;
case Command.Set:
// set <var name> <property> <value>
name = instruction.Fields[0];
property = instruction.Fields[1];
Vars.TryGetValue(name, out var);
if (var != null)
{
if (var is List <IMyTerminalBlock> )
{
blocks = (List <IMyTerminalBlock>)var;
if (blocks.Count > 0)
{
IMyTerminalBlock firstBlock = blocks[0];
ITerminalProperty prop = firstBlock.GetProperty(property);
//myProgram.drawingSurface.WriteText($"\nProperty={prop.TypeName}", true);
switch (prop.TypeName)
{
case "Single":
ParseFieldFloat(instruction.Fields[2], out value1);
break;
case "Boolean":
ParseFieldBool(instruction.Fields[2], out bool1);
break;
}
blocks.ForEach(delegate(IMyTerminalBlock block)
{
switch (prop.TypeName)
{
case "Single":
block.SetValueFloat(property, value1);
break;
case "Boolean":
block.SetValueBool(property, bool1);
break;
}
});
}
}
}
Line++;
break;
case Command.Get:
// get <var name> <property> <var name>
name = instruction.Fields[0];
property = instruction.Fields[1];
source = instruction.Fields[2];
Vars.TryGetValue(source, out var);
if (var != null)
{
if (var is List <IMyTerminalBlock> )
{
blocks = (List <IMyTerminalBlock>)var;
if (blocks.Count > 0)
{
IMyTerminalBlock firstBlock = blocks[0];
ITerminalProperty prop = firstBlock.GetProperty(property);
switch (prop.TypeName)
{
case "Single":
Vars.Add(name, firstBlock.GetValueFloat(property));
break;
case "Boolean":
Vars.Add(name, firstBlock.GetValueBool(property));
break;
}
}
}
}
Line++;
break;
case Command.Wait:
// wait <var name> <property> <value> <epsilon>
name = instruction.Fields[0];
property = instruction.Fields[1];
if (instruction.Fields.Count > 3)
{
ParseFieldFloat(instruction.Fields[3], out epsilon);
}
Vars.TryGetValue(name, out var);
bool isState = true;
if (var != null)
{
if (var is List <IMyTerminalBlock> )
{
blocks = (List <IMyTerminalBlock>)var;
if (blocks.Count > 0)
{
ITerminalProperty prop = null;
IMyTerminalBlock firstBlock = blocks[0];
prop = firstBlock.GetProperty(property);
if (prop == null)
{
switch (GetAttibutType(property))
{
case "Single":
ParseFieldFloat(instruction.Fields[2], out value1);
break;
case "Boolean":
ParseFieldBool(instruction.Fields[2], out bool1);
break;
}
}
else
{
switch (prop.TypeName)
{
case "Single":
ParseFieldFloat(instruction.Fields[2], out value1);
break;
case "Boolean":
ParseFieldBool(instruction.Fields[2], out bool1);
break;
}
break;
}
blocks.ForEach(delegate(IMyTerminalBlock block)
{
if (prop == null)
{
switch (GetAttibutType(property))
{
case "Single":
value2 = GetAttibutFloat(block, property);
if (Math.Abs(value2 - value1) > epsilon)
{
isState = false;
}
break;
case "Boolean":
bool2 = GetAttibutBool(block, property);
if (bool2 != bool1)
{
isState = false;
}
break;
}
}
else
{
switch (prop.TypeName)
{
case "Single":
value2 = block.GetValueFloat(property);
if (Math.Abs(value2 - value1) > epsilon)
{
isState = false;
}
break;
case "Boolean":
bool2 = block.GetValueBool(property);
if (bool2 != bool1)
{
isState = false;
}
break;
}
}
});
}
}
}
if (isState)
{
Line++;
}
break;
}
}
