Esempio n. 1
0
            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;
            }
Esempio n. 2
0
            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;
            }
Esempio n. 3
0
            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;
                }
            }