public Program()  // init
        {
            this.gts = GridTerminalSystem;

            //init mechanical blocks
            this.legs = new Dictionary <string, Leg>()
            {
                ["right"] = new Leg(this.gts, "Right"),
                ["left"]  = new Leg(this.gts, "Left")
            };
            this.cockpit         = this.gts.GetBlockWithName("LWE Cockpit") as IMyCockpit;
            this.cockpitLcdLarge = Me.GetSurface(0);

            this.activeMovementAction = null;

            if (!(this.legs["right"].checkValidity() && this.legs["left"].checkValidity() && this.cockpit != null))
            {
                this.cockpitLcdLarge.WriteText("one ore more of the expected mechanical components not found!\n");
                Echo("one ore more of the expected mechanical components not found!\n");
            }
            else
            {
                this.cockpitLcdLarge.WriteText("all set up!\n");
                Echo("all set up!\n");
            }
        }
예제 #2
0
    void Start()
    {
        text.SetActive(false);
        initHealth = health;

        //INCONTROL Grabbing leg child for INCONTROL inputs
        legScript = this.gameObject.transform.GetChild(0).GetComponent <LegMovement>();
    }
예제 #3
0
    public LegMovement getNextMove()
    {
        LegMovement retVal = new LegMovement();

        if (queue.Count > 0)
        {
            retVal       = queue[0];
            retVal.valid = true;
            queue.RemoveAt(0);
        }

        return(retVal);
    }
예제 #4
0
    public static SequenceFrame newSequenceFrame(Dictionary <int, LegMovement> movements, int miliseconds = -1)
    {
        SequenceFrame retVal = new SequenceFrame();

        if (miliseconds == -1)
        {
            float maxTime = 0f;
            foreach (KeyValuePair <int, LegMovement> k in movements)
            {
                maxTime = Mathf.Max(maxTime, k.Value.maxExecTime);
            }
            retVal.maxMaxExecTime = maxTime;
        }
        else
        {
            retVal.maxMaxExecTime = miliseconds / 1000f;
        }


        Dictionary <int, LegMovement> scaledMoves = new Dictionary <int, LegMovement>();

        foreach (KeyValuePair <int, LegMovement> mov in movements)
        {
            float scaleFactor = mov.Value.maxExecTime / retVal.maxMaxExecTime;

            if (retVal.maxMaxExecTime == 0)
            {
                scaleFactor = 0;
            }


            LegMovement newMov = new LegMovement();
            newMov.coxa  = mov.Value.coxa;
            newMov.tibia = mov.Value.tibia;
            newMov.femur = mov.Value.femur;

            newMov.coxaSpeed   = mov.Value.coxaSpeed * scaleFactor;
            newMov.tibiaSpeed  = mov.Value.tibiaSpeed * scaleFactor;
            newMov.maxExecTime = retVal.maxMaxExecTime;

            scaledMoves.Add(mov.Key, newMov);
        }

        retVal.movements = scaledMoves;

        return(retVal);
    }
예제 #5
0
 // Use this for initialization
 void Start()
 {
     leg = FindObjectOfType <LegMovement>();
 }
        public void Main(string argument, UpdateType updateSource)
        {
            if (argument != "")
            {
                if (argument == "start")
                {
                    Runtime.UpdateFrequency = UpdateFrequency.Update1;
                }
                else if (argument == "stop")
                {
                    if (this.activeMovementAction != null)
                    {
                        this.activeMovementAction.StopMovement();
                        this.activeMovementAction = null;
                    }

                    Runtime.UpdateFrequency = UpdateFrequency.None;

                    this.cockpitLcdLarge.WriteText("movement script stopped", true);
                    return;
                }
            }

            // is there an active action? then continue it
            if (this.activeMovementAction != null)
            {
                if (!this.activeMovementAction.Update())  // Update() continues the action and returns false on finish
                {
                    // active action finished
                    this.activeMovementAction = null;

                    // after finished action update walk state
                    ReadCustomData();
                    if (Convert.ToInt32(customData["walkState"]) >= 36)
                    {
                        customData["walkState"] = "1";  // start cycle at state "1". "0" could be used as initial state

                        // switch Legs
                        var activeLeg = customData["walkActiveLeg"];
                        customData["walkActiveLeg"]   = customData["walkInactiveLeg"];
                        customData["walkInactiveLeg"] = activeLeg;
                    }
                    else
                    {
                        if (customData["walkState"] == "0")
                        {
                            Runtime.UpdateFrequency = UpdateFrequency.None;
                            this.cockpitLcdLarge.WriteText("Script Stopped after setup phase");
                        }
                        customData["walkState"] = (Convert.ToInt32(customData["walkState"]) + 1).ToString();
                    }
                    WriteCustomData();
                }
            }
            else
            {
                // choose and set up new active movement action
                ReadCustomData();
                var  activeLeg      = this.legs[customData["walkActiveLeg"]];
                var  inactiveLeg    = this.legs[customData["walkInactiveLeg"]];
                bool rightLegActive = customData["walkActiveLeg"] == "right";

                this.cockpitLcdLarge.WriteText("walk state : " + customData["walkActiveLeg"] + " " + customData["walkState"] + "\n");

                if (!(new List <string>()
                {
                    "straight", "gps"
                }.Contains(customData["targetMode"])))
                {
                    this.cockpitLcdLarge.WriteText("ERROR: unknown target mode\n");
                    Runtime.UpdateFrequency = UpdateFrequency.None;
                    return;
                }

                switch (Convert.ToInt32(customData["walkState"]))
                {
                case 0:      // initial state. not reached in normal execution
                    this.activeMovementAction = new PistonAndLandingGearExtendAndLock(activeLeg.pistons["down"], activeLeg.landingGear, activeLeg.pistons["down support"]);
                    break;

                case 1:
                    this.activeMovementAction = new UnlockSafe(inactiveLeg.landingGear, activeLeg.landingGear);
                    break;

                case 2:
                    this.activeMovementAction = new PistonMoveFull(inactiveLeg.pistons["down"], false, inactiveLeg.pistons["down support"]);
                    break;

                case 3:
                    this.activeMovementAction = new PistonMoveFull(inactiveLeg.pistons["forward"], false, inactiveLeg.pistons["forward support"]);
                    break;

                case 4:
                    this.activeMovementAction = new PistonAndLandingGearExtendAndLock(inactiveLeg.pistons["down"], inactiveLeg.landingGear, inactiveLeg.pistons["down support"]);
                    break;

                case 5:
                    this.activeMovementAction = new UnlockSafe(activeLeg.landingGear, inactiveLeg.landingGear);
                    break;

                case 6:
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["down"], false, activeLeg.pistons["down support"]);
                    break;

                case 7:
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["forward"], false, activeLeg.pistons["forward support"]);
                    break;

                case 8:
                    this.activeMovementAction = new PistonAndLandingGearExtendAndLock(activeLeg.pistons["down"], activeLeg.landingGear, activeLeg.pistons["down support"]);
                    break;

                case 9:
                    activeLeg.mergeBlocks["foot"].ApplyAction("OnOff_Off");
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["down support"], false);
                    break;

                case 10:
                    activeLeg.mergeBlocks["knee"].ApplyAction("OnOff_Off");
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["forward support"], false);
                    break;

                case 11:
                    inactiveLeg.mergeBlocks["knee"].ApplyAction("OnOff_Off");
                    this.activeMovementAction = new PistonMoveFull(inactiveLeg.pistons["forward support"], false);
                    break;

                case 12:      // set new Yaw
                    if (customData["targetMode"] == "straight")
                    {
                        this.activeMovementAction = new SetBodyAxis(activeLeg.pistons["forward"], inactiveLeg.pistons["forward"], inactiveLeg.rotors["yaw 1"],
                                                                    activeLeg.rotors["yaw 1"], activeLeg.rotors["yaw 2"], -1 * Convert.ToDouble(customData["addYaw"]) * 2 * Math.PI / 360, rightLegActive);
                    }
                    else if (customData["targetMode"] == "gps")
                    {
                        throw new System.Exception("not implemented");
                    }
                    else
                    {
                        throw new System.Exception($"unknown target mode: {customData["targetMode"]}");
                    }
                    break;

                case 13:      // set new Pitch
                    if (customData["targetMode"] == "straight")
                    {
                        this.activeMovementAction = new SetBodyAxis(activeLeg.pistons["down"], inactiveLeg.pistons["down"], inactiveLeg.rotors["roll 1"],
                                                                    activeLeg.rotors["roll 1"], activeLeg.rotors["roll 2"], inactiveLeg.rotors["roll 1"].Angle - 1 * GetRoll(), !rightLegActive, inactiveLeg.pistons["down support"]);
                    }
                    else if (customData["targetMode"] == "gps")
                    {
                        throw new System.Exception("not implemented");
                    }
                    else
                    {
                        throw new System.Exception($"unknown target mode: {customData["targetMode"]}");
                    }
                    break;

                case 14:
                    var pitchCorrection = Convert.ToDouble(customData["setPitch"]) * 2 * Math.PI / 360 - GetPitch();
                    if (!rightLegActive)
                    {
                        pitchCorrection *= -1;
                    }

                    this.activeMovementAction = new SetBodyAxis(activeLeg.pistons["down"], inactiveLeg.pistons["down"], inactiveLeg.rotors["pitch 1"],
                                                                activeLeg.rotors["pitch 2"], activeLeg.rotors["pitch 3"], pitchCorrection, !rightLegActive, inactiveLeg.pistons["down support"]);
                    break;

                case 15:
                    this.activeMovementAction = new UnlockSafe(activeLeg.landingGear, inactiveLeg.landingGear);
                    break;

                case 16:
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["down"], false);
                    break;

                case 17:
                    this.activeMovementAction = new PistonMoveFull(activeLeg.pistons["forward"], false);
                    break;

                case 18:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["roll 2"], 0);
                    break;

                case 19:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["pitch 3"], 0);
                    break;

                case 20:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["pitch 2"], 0);
                    break;

                case 21:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["yaw 2"], 0);
                    break;

                case 22:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["roll 1"], 0);
                    break;

                case 23:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["pitch 1"], 0);
                    break;

                case 24:
                    this.activeMovementAction = new RotorPositionSet(activeLeg.rotors["yaw 1"], 0);
                    break;

                case 25:
                    this.activeMovementAction = new PistonAndMergeBlockExtendAndMerge(activeLeg.pistons["forward support"], activeLeg.mergeBlocks["knee"], this.gts);
                    break;

                case 26:
                    this.activeMovementAction = new PistonAndMergeBlockExtendAndMerge(activeLeg.pistons["down support"], activeLeg.mergeBlocks["foot"], this.gts);
                    break;

                case 27:
                    this.activeMovementAction = new PistonAndLandingGearExtendAndLock(activeLeg.pistons["down"], activeLeg.landingGear, activeLeg.pistons["down support"]);
                    break;

                case 28:
                    this.activeMovementAction = new UnlockSafe(inactiveLeg.landingGear, activeLeg.landingGear);
                    break;

                case 29:
                    this.activeMovementAction = new PistonMoveFull(inactiveLeg.pistons["down"], false, inactiveLeg.pistons["down support"]);
                    break;

                case 30:
                    this.activeMovementAction = new PistonMoveFull(inactiveLeg.pistons["forward"], false);
                    break;

                case 31:
                    this.activeMovementAction = new RotorPositionSet(inactiveLeg.rotors["roll 1"], 0);
                    break;

                case 32:
                    this.activeMovementAction = new RotorPositionSet(inactiveLeg.rotors["pitch 1"], 0);
                    break;

                case 33:
                    this.activeMovementAction = new RotorPositionSet(inactiveLeg.rotors["yaw 1"], 0);
                    break;

                case 34:
                    this.activeMovementAction = new PistonAndMergeBlockExtendAndMerge(inactiveLeg.pistons["forward support"], inactiveLeg.mergeBlocks["knee"], this.gts);
                    break;

                case 35:
                    this.activeMovementAction = new PistonAndLandingGearExtendAndLock(inactiveLeg.pistons["down"], inactiveLeg.landingGear, inactiveLeg.pistons["down support"]);
                    break;

                case 36:
                    // intensive drilling action (mabe add slow/ adaptive drilling and battery checks here)
                    this.activeMovementAction = new PistonQuadrupleExtend(activeLeg.pistons["forward"], activeLeg.pistons["forward support"],
                                                                          inactiveLeg.pistons["forward"], inactiveLeg.pistons["forward support"]);
                    break;
                }
            }
        }
예제 #7
0
    // Update is called once per frame
    void Update()
    {
        if (!shouldAnimate)
        {
            return;
        }

        //Debug.Log("Anim: " + (cFinish - cStart));
        //Debug.Log("Anim: " + ((Time.time - cStart) / (cFinish - cStart)));
        //Debug.Log("Anim: " + (Time.time - startTime) );
        float ratio = 0f;

        if (maxExecTime == 0)
        {
            ratio = 1.1f;
        }
        else
        {
            ratio = ((Time.time - startTime) / maxExecTime);
        }

        if (ratio <= 1)
        {
            //Debug.Log("Anim: " + ((Time.time - cStart) / (cFinish - cStart)));
            if (!emptyMove)
            {
                //Mathf.LerpAngle()
                Quaternion newCoxa = Quaternion.Lerp(lastCoxa, cTarget, ratio);
                Coxa.transform.localRotation = newCoxa;
            }

            if (!emptyMove)
            {
                Quaternion newFemur = Quaternion.Lerp(lastFemur, fTarget, ratio);
                Femur.transform.localRotation = newFemur;
            }

            if (!emptyMove)
            {
                Quaternion newTibia = Quaternion.Lerp(lastTibia, tTarget, ratio);
                Tibia.transform.localRotation = newTibia;
            }
        }
        else
        {
            LegMovement nextMove = getNextMove();
            if (nextMove.valid)
            {
                maxExecTime = nextMove.maxExecTime;
                startTime   = Time.time;
                if (!nextMove.empty)
                {
                    this.Coxa.localRotation  = cTarget;
                    this.Femur.localRotation = fTarget;
                    this.Tibia.localRotation = tTarget;

                    //Debug.Log("New Move: " + nextMove.coxa + ", " + fixAngle(nextMove.femur) + ", " + nextMove.tibia + " EXECTIME: " + nextMove.maxExecTime);
                    setCoxa(nextMove.coxa, nextMove.coxaSpeed);
                    setFemur(nextMove.femur, nextMove.femurSpeed);
                    setTibia(nextMove.tibia, nextMove.tibiaSpeed);
                    emptyMove = false;
                }
                else
                {
                    emptyMove = true;
                }
            }
        }
    }
예제 #8
0
 public void addMove(LegMovement lm)
 {
     queue.Add(lm);
 }