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");
}
}
void Start()
{
text.SetActive(false);
initHealth = health;
//INCONTROL Grabbing leg child for INCONTROL inputs
legScript = this.gameObject.transform.GetChild(0).GetComponent <LegMovement>();
}
public LegMovement getNextMove()
{
LegMovement retVal = new LegMovement();
if (queue.Count > 0)
{
retVal = queue[0];
retVal.valid = true;
queue.RemoveAt(0);
}
return(retVal);
}
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);
}
// 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;
}
}
}
// 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;
}
}
}
}
public void addMove(LegMovement lm)
{
queue.Add(lm);
}
