double GetAverageArmSegmentLength(RoboticArm arm)
{
var segment1Length = Vector3D.Distance(arm.Rotor1.Rotor.GetPosition(), arm.Rotor2.Rotor.GetPosition());
var segment2Length = Vector3D.Distance(arm.Rotor2.Rotor.GetPosition(), arm.Tip.GetPosition());
return((segment1Length + segment2Length) / 2);
}
public Crawler(RoboticArm leftBack, RoboticArm leftFront, RoboticArm rightBack, RoboticArm rightFront, IMyRemoteControl rc)
{
LeftBack = leftBack;
LeftFront = leftFront;
RightBack = rightBack;
RightFront = rightFront;
Rc = rc;
StepIndent = new []
{
GetAverageArmSegmentLength(LeftBack),
GetAverageArmSegmentLength(LeftFront),
GetAverageArmSegmentLength(RightBack),
GetAverageArmSegmentLength(RightFront),
}.Average() *StepIndentMultiplier;
// getting local positions of leg base rotation rotors
var lb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftBack.RotorRotation.Rotor.GetPosition());
var lf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftFront.RotorRotation.Rotor.GetPosition());
var rb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightBack.RotorRotation.Rotor.GetPosition());
var rf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightFront.RotorRotation.Rotor.GetPosition());
Center = VectorUtility.GetAverageVector(
VectorUtility.GetAverageVector(lb, lf),
VectorUtility.GetAverageVector(rb, rf));
lb -= Center;
lf -= Center;
rb -= Center;
rf -= Center;
LbMarchPoint = VectorUtility.Normalize(lb) * (StepIndent + lb.Length());
LfMarchPoint = VectorUtility.Normalize(lf) * (StepIndent + lf.Length());
RbMarchPoint = VectorUtility.Normalize(rb) * (StepIndent + rb.Length());
RfMarchPoint = VectorUtility.Normalize(rf) * (StepIndent + rf.Length());
Lbh = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 0.5, LbMarchPoint.Z);
Lfh = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 0.5, LfMarchPoint.Z);
Rbh = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 0.5, RbMarchPoint.Z);
Rfh = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 0.5, RfMarchPoint.Z);
Lbs = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 1.5, LbMarchPoint.Z + 3);
Lfs = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 1.5, LfMarchPoint.Z - 3);
Rbs = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 1.5, RbMarchPoint.Z + 3);
Rfs = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 1.5, RfMarchPoint.Z - 3);
}
public Program()
{
var allBlocks = new List <IMyTerminalBlock>();
GridTerminalSystem.GetBlocks(allBlocks);
Lcd = GetBlock <IMyTextSurface>(allBlocks, x => x.CustomName == "lcd controls");
C**k = GetBlock <IMyRemoteControl>(allBlocks, x => x.CustomName.Contains("[ra c]"));
Tip = GetBlock <IMyTerminalBlock>(allBlocks, x => x.CustomName.Contains("[ra t]"));
var rotationRotor = GetBlock <IMyMotorStator>(allBlocks, x => x.CustomName.Contains("[ra r]"));
roboticArm = new RoboticArm(allBlocks, rotationRotor, Tip);
roboticArm.lcd = Lcd;
Lcd.WriteText("Hello world!");
Runtime.UpdateFrequency = UpdateFrequency.Update10;
}
bool NearDest(RoboticArm arm, Vector3D dest) => Vector3D.Distance(arm.Tip.GetPosition(), dest) < 0.3;