Example #1
0
      public void Main(string argument, UpdateType updateSource)
      {
          if (!ModuleInited())
          {
              return;
          }
          if (init_error)
          {
              Echo(init_message);
              ProgBlockLCD.WriteText(init_message);
              if (LCD != null)
              {
                  LCD.WriteText(init_message);
              }
              return;
          }

          ToLog("", false, false);

          if (!string.IsNullOrEmpty(argument))
          {
              string cmd, cmd_param;
              string argument_norm = argument.Trim().ToLower();
              int    spc_idx       = argument_norm.IndexOf(" ");
              if (spc_idx == -1)
              {
                  cmd       = argument_norm;
                  cmd_param = string.Empty;
              }
              else
              {
                  cmd       = argument_norm.Substring(0, spc_idx);
                  cmd_param = argument_norm.Substring(spc_idx + 1).TrimStart();
              }

              switch (cmd)
              {
              case "on":
              {
                  HardwareOn();
                  break;
              }

              case "off":
              {
                  HardwareOff();
                  break;
              }

              case "begin":
              {
                  GotoStartPos();
                  break;
              }

              case "drill":
              {
                  StartDrill();
                  break;
              }

              case "home":
              {
                  GotoHome();
                  break;
              }

              case "pause":
              {
                  PauseWork();
                  break;
              }

              case "continue":
              {
                  ContinueWork();
                  break;
              }

              case "save":
              {
                  switch (cmd_param)
                  {
                  case "l":
                  case "left":
                  {
                      FullStop();
                      angle_min = CurrentAngle();
                      break;
                  }

                  case "r":
                  case "right":
                  {
                      FullStop();
                      angle_max = CurrentAngle();
                      break;
                  }

                  case "b":
                  case "back":
                  {
                      FullStop();
                      radius_min = CurrentRadius();
                      break;
                  }

                  case "f":
                  case "forward":
                  {
                      FullStop();
                      radius_max = CurrentRadius();
                      break;
                  }

                  case "u":
                  case "up":
                  {
                      FullStop();
                      depth_min = CurrentDepth();
                      break;
                  }

                  case "d":
                  case "down":
                  {
                      FullStop();
                      depth_max = CurrentDepth();
                      break;
                  }

                  default:
                      break;
                  }
                  break;
              }

              case "move":
              {
                  switch (cmd_param)
                  {
                  case "l":
                  case "left":
                  {
                      FullStop();
                      MoveLeft();
                      break;
                  }

                  case "r":
                  case "right":
                  {
                      FullStop();
                      MoveRight();
                      break;
                  }

                  case "f":
                  case "forward":
                  {
                      FullStop();
                      MoveForward();
                      break;
                  }

                  case "b":
                  case "back":
                  {
                      FullStop();
                      MoveBack();
                      break;
                  }

                  case "u":
                  case "up":
                  {
                      FullStop();
                      MoveUp();
                      break;
                  }

                  case "d":
                  case "down":
                  {
                      FullStop();
                      MoveDown();
                      break;
                  }

                  default:
                  {
                      FullStop();
                      break;
                  }
                  }
                  break;
              }

              case "clear":
                  FullStop();
                  ClearPrgSettings();
                  break;

              case "stop":
                  FullStop();
                  break;

              case "chdir":
              {
                  switch (cmd_param)
                  {
                  case "r":
                  case "radius":
                  {
                      radius_direction = !radius_direction;
                      break;
                  }

                  case "a":
                  case "angle":
                  {
                      turn_direction = !turn_direction;
                      break;
                  }

                  default:
                  {
                      break;
                  }
                  }
                  break;
              }

              default:
                  break;
              }
              return;
          }

          if ((updateSource & UpdateType.Update10) == 0)
          {
              return;
          }

          //if (control_locked && Cockpit != null)
          //{
          //    if (Cockpit.MoveIndicator.X > 0f)
          //    {
          //        DesiredAngle = MAX_ANGLE;
          //        SetAngleSpeed(FREE_VELOCITY);
          //    }
          //    else if (Cockpit.MoveIndicator.X < 0f)
          //    {
          //        DesiredAngle = -MAX_ANGLE;
          //        SetAngleSpeed(FREE_VELOCITY);
          //    }
          //    else
          //    {
          //        SetAngleSpeed(0f);
          //    }
          //}

          cargo_check_counter += 1;
          if (cargo_check_counter >= CARGO_CHECK_INTERVAL)
          {
              cargo_check_counter = 0;
              CheckCargo();

              if (CurrentProgramm == DrillProgrammEnum.Drill)
              {
                  if (cargo_fullness_percentage >= STOP_PERCENTAGE)
                  {
                      WaitCargoFull();
                  }
              }
              else if (CurrentProgramm == DrillProgrammEnum.WaitFull)
              {
                  if (cargo_fullness_percentage <= CONTINUE_PERCENTAGE)
                  {
                      ContinueWork();
                  }
              }
          }

          if (CheckProgramm())
          {
              switch (CurrentProgramm)
              {
              case DrillProgrammEnum.Drill:
              {
                  GotoHome();
                  break;
              }

              case DrillProgrammEnum.GotoHome:
              {
                  HardwareOff();
                  break;
              }

              case DrillProgrammEnum.GotoBegin:
              case DrillProgrammEnum.Move:
              {
                  FullStop();
                  break;
              }
              }
          }

          ToLog(string.Format("Тек.верт {0:#0.00} / {1:#0.00}", CurrentDepth(), DesiredDepth));
          ToLog(string.Format("Отклонение {0:#0.00}", DepthDeviation()));
          ToLog(string.Format("Верт.скорость {0:#0.00}", vert_spd));
          ToLog(string.Format("Скорость поршня {0:#0.00}", VerticalPistons[0].Velocity));
          ToLog(string.Format("Поз. поршня {0:#0.00}", VerticalPistons[0].CurrentPosition));

          VerticalPistons.ForEach(b => ToLog(string.Format("Длина/скор {0:#0.00000} / {1:#0.00000}", b.CurrentPosition, b.Velocity)));
          ToLog(string.Format("Поз. ротора {0:#0.00000}", CurrentAngle()));

          DisplayUpdateCounter += 1;
          if (DisplayUpdateCounter >= DisplayUpdateInterval)
          {
              ProgBlockLCD.WriteText("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n");
              Echo("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n" + init_message);
              DisplayUpdateCounter = 0;
              runTime = 0d;

              ProgBlockLCD.WriteText("Программа: " + ProgramToString() + "\n", true);
              ProgBlockLCD.WriteText("Состояние: " + StateToString() + "\n", true);

              ProgBlockLCD.WriteText(string.Format("Инвентарь: {0:#0.0}%\n", cargo_fullness_percentage), true);

              ProgBlockLCD.WriteText("=== Задано ===\n", true);
              ProgBlockLCD.WriteText(string.Format("Радиус: {0:#0.0} / {1:#0.0}\n", radius_min, radius_max), true);
              ProgBlockLCD.WriteText(string.Format("Глубина: {0:#0.0} / {1:#0.0}\n", depth_min, depth_max), true);
              ProgBlockLCD.WriteText(string.Format("Угол: {0:#0} / {1:#0}\n", MathHelper.ToDegrees(angle_min), MathHelper.ToDegrees(angle_max)), true);
              ProgBlockLCD.WriteText(string.Format("Направление: {0} / {1}\n", (turn_direction ? "вправо" : "влево"), (radius_direction ? "вперёд" : "назад")), true);

              ProgBlockLCD.WriteText("=== Положение ===\n", true);
              ProgBlockLCD.WriteText(string.Format("Радиус: {0:#0.0} / {1:#0.0}\n", CurrentRadius(), DesiredRadius), true);
              ProgBlockLCD.WriteText(string.Format("Глубина: {0:#0.0} / {1:#0.0}\n", CurrentDepth(), DesiredDepth), true);
              ProgBlockLCD.WriteText(string.Format("Угол: {0:#0} / {1:#0}\n", MathHelper.ToDegrees(CurrentAngle()), MathHelper.ToDegrees(DesiredAngle)), true);

              if (LCD != null)
              {
                  string t = ProgBlockLCD.GetText();
                  LCD.WriteText(t);
              }
          }
          runTime += Runtime.LastRunTimeMs;
      }
Example #2
0
        public void Main(string argument, UpdateType updateSource)
        {
            if (!ModuleInited())
            {
                return;
            }
            if (init_error)
            {
                Echo(init_message);
                ProgBlockLCD.WriteText(init_message);
                if (LCD != null)
                {
                    LCD.WriteText(init_message);
                }
                return;
            }

            ToLog("", false, false);

            Vector3D GravityVector = Cockpit.GetNaturalGravity();
            Vector3D GravityNorm   = Vector3D.Normalize(GravityVector);

            Vector3D VelocityVector     = Cockpit.GetShipVelocities().LinearVelocity;
            Vector3D VelocityVectorNorm = Vector3D.Normalize(VelocityVector);

            Vector3D ForwardVectorNorm = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Forward, GravityNorm));
            Vector3D LeftVectorNorm    = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Left, GravityNorm));

            Vector3D ForwardInput = ForwardVectorNorm * Cockpit.MoveIndicator.Z;
            Vector3D LeftInput    = LeftVectorNorm * Cockpit.MoveIndicator.X;

            Vector3D ForwardVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref ForwardVectorNorm);
            Vector3D LeftVelocity    = Vector3D.ProjectOnVector(ref VelocityVector, ref LeftVectorNorm);

            float UpInput = Cockpit.MoveIndicator.Y / 10;

            Vector3D AlignVector = Vector3D.Zero;

            /*if (!string.IsNullOrEmpty(argument))
             * {
             *  string cmd, cmd_param;
             *  string argument_norm = argument.Trim().ToLower();
             *  int spc_idx = argument_norm.IndexOf(" ");
             *  if (spc_idx == -1)
             *  {
             *      cmd = argument_norm;
             *      cmd_param = string.Empty;
             *  }
             *  else
             *  {
             *      cmd = argument_norm.Substring(0, spc_idx);
             *      cmd_param = argument_norm.Substring(spc_idx + 1).TrimStart();
             *  }*/

            switch (argument)
            {
            case "Dampeners":
            {
                PilotMode = PilotModeEnum.Dampeners;
                break;
            }

            case "Cruise":
            {
                PilotMode = PilotModeEnum.Cruise;
                if (ForwardVectorNorm == Vector3D.Normalize(ForwardVelocity))
                {
                    cruisespeed = -(float)ForwardVelocity.Length();
                }
                else
                {
                    cruisespeed = (float)ForwardVelocity.Length();
                }
                break;
            }

            case "FreeMove":
            {
                PilotMode = PilotModeEnum.FreeMove;
                break;
            }

            case "Drill":
            {
                PilotMode   = PilotModeEnum.Cruise;
                cruisespeed = 3f;
                altitude    = 4f;
                foreach (IMyThrust hover in HoverList)
                {
                    hover.SetValue <float>("altitudemin_slider_L", altitude);
                }
                break;
            }

            case "Down":
            {
                PilotMode   = PilotModeEnum.Cruise;
                cruisespeed = 5f;
                altitude    = 5f;
                foreach (IMyThrust hover in HoverList)
                {
                    hover.SetValue <float>("altitudemin_slider_L", altitude);
                }
                break;
            }

            default:
                break;
            }
            //}

            if (!Vector3D.IsZero(LeftInput))
            {
                AlignVector += LeftInput * KI;
            }
            else
            if (PilotMode != PilotModeEnum.FreeMove)
            {
                AlignVector += LeftVelocity * KV;
            }

            if (!Vector3D.IsZero(ForwardInput))
            {
                if (PilotMode == PilotModeEnum.Cruise)
                {
                    cruisespeed += -Cockpit.MoveIndicator.Z / 10;
                    AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
                }
                else
                {
                    AlignVector += ForwardInput * KI;
                }
            }
            else
            if (PilotMode == PilotModeEnum.Cruise)
            {
                AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
            }
            else
            if (PilotMode != PilotModeEnum.FreeMove)
            {
                AlignVector += ForwardVelocity * KV;
            }
            AlignVector += GravityNorm;



            float PitchInput = -(float)AlignVector.Dot(Cockpit.WorldMatrix.Forward);
            float RollInput  = (float)AlignVector.Dot(Cockpit.WorldMatrix.Left);
            float YawInput   = Cockpit.RollIndicator;

            foreach (IMyThrust hover in HoverList)
            {
                altitude  = hover.GetValue <float>("altitudemin_slider_L");
                altitude += UpInput;
                hover.SetValue <float>("altitudemin_slider_L", altitude);
            }

            foreach (IMyGyro gyro in GyroList)
            {
                gyro.GyroOverride = true;
                gyro.Yaw          = YawInput;
                gyro.Roll         = RollInput;
                gyro.Pitch        = PitchInput;
            }
            DisplayUpdateCounter += 1;
            if (DisplayUpdateCounter >= DisplayUpdateInterval)
            {
                ProgBlockLCD.WriteText("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n");
                Echo("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n" + init_message);
                DisplayUpdateCounter = 0;
                runTime = 0d;

                Cockpit.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out SealevelAlt);
                Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out SurfaceAlt);
                ProgBlockLCD.WriteText("Режим: " + PilotModeToString() + "\n", true);
                ProgBlockLCD.WriteText(string.Format("cruisespeed: {0:#0.0}\n", cruisespeed), true);
                ProgBlockLCD.WriteText(string.Format("FV: {0:#0.0} / LV: {1:#0.0} / V: {2:#0.0}\n", ForwardVelocity.Length(), LeftVelocity.Length(), VelocityVector.Length()), true);
                ProgBlockLCD.WriteText(string.Format("Alt: {0:#0.0} / SLA: {1:#0.0} / SFA: {2:#0.0}\n", altitude, SealevelAlt, SurfaceAlt), true);

                if (LCD != null)
                {
                    string t = ProgBlockLCD.GetText();
                    LCD.WriteText(t);
                }
            }
            runTime += Runtime.LastRunTimeMs;
        }
Example #3
0
        public void Main(string argument, UpdateType updateSource)
        {
            if (!ModuleInited())
            {
                return;
            }
            if (init_error)
            {
                Echo(init_message);
                ProgBlockLCD.WriteText(init_message);
                if (LCD != null)
                {
                    LCD.WriteText(init_message);
                }
                return;
            }

            ToLog("", false, false);

            Vector3D GravityVector = Cockpit.GetNaturalGravity();
            Vector3D GravityNorm   = Vector3D.Normalize(GravityVector);

            Vector3D VelocityVector     = Cockpit.GetShipVelocities().LinearVelocity;
            Vector3D VelocityVectorNorm = Vector3D.Normalize(VelocityVector);

            Vector3D ForwardVectorNorm = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Forward, GravityNorm));
            Vector3D LeftVectorNorm    = Vector3D.Normalize(ForwardVectorNorm.Cross(GravityNorm));

            Vector3D ForwardInput = ForwardVectorNorm * Cockpit.MoveIndicator.Z;
            Vector3D LeftInput    = LeftVectorNorm * Cockpit.MoveIndicator.X;

            Vector3D ForwardVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref ForwardVectorNorm);
            Vector3D LeftVelocity    = Vector3D.ProjectOnVector(ref VelocityVector, ref LeftVectorNorm);

            float UpInput = Cockpit.RollIndicator / 10;

            Vector3D AlignVector = Vector3D.Zero;

            switch (argument)
            {
            case "Dampeners":
            {
                PilotMode = PilotModeEnum.Dampeners;
                break;
            }

            case "Cruise":
            {
                PilotMode = PilotModeEnum.Cruise;
                if ((ForwardVectorNorm + ForwardVelocity).Length() > ForwardVelocity.Length())
                {
                    cruisespeed = (float)ForwardVelocity.Length();
                }
                else
                {
                    cruisespeed = -(float)ForwardVelocity.Length();
                }
                break;
            }

            case "FreeMove":
            {
                PilotMode = PilotModeEnum.FreeMove;
                break;
            }

            default:
                break;
            }

            if (!Vector3D.IsZero(LeftInput))
            {
                AlignVector += LeftInput * KI;
            }
            else
            if (PilotMode != PilotModeEnum.FreeMove)
            {
                AlignVector += LeftVelocity * KV;
            }

            if (!Vector3D.IsZero(ForwardInput))
            {
                if (PilotMode == PilotModeEnum.Cruise)
                {
                    cruisespeed += -Cockpit.MoveIndicator.Z / 10;
                    AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
                }
                else
                {
                    AlignVector += ForwardInput * KI;
                }
            }
            else
            if (PilotMode == PilotModeEnum.Cruise)
            {
                AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
            }
            else
            if (PilotMode != PilotModeEnum.FreeMove)
            {
                AlignVector += ForwardVelocity * KV;
            }
            AlignVector += GravityNorm;



            float PitchInput = -(float)AlignVector.Dot(Cockpit.WorldMatrix.Forward);
            float RollInput  = (float)AlignVector.Dot(Cockpit.WorldMatrix.Left);
            float YawInput   = Cockpit.RotationIndicator.Y;

            foreach (IMyThrust hover in HoverList)
            {
                altitude  = hover.GetValue <float>("altitudemin_slider_L");
                altitude += UpInput;
                hover.SetValue <float>("altitudemin_slider_L", altitude);
            }

            foreach (IMyGyro gyro in GyroList)
            {
                gyro.GyroOverride = true;
                gyro.Yaw          = YawInput;
                gyro.Roll         = RollInput;
                gyro.Pitch        = PitchInput;
            }
            DisplayUpdateCounter += 1;
            if (DisplayUpdateCounter >= DisplayUpdateInterval)
            {
                ProgBlockLCD.WriteText("\n" + "\n" + "\n" + "Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n");
                Echo("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n" + init_message);
                DisplayUpdateCounter = 0;
                runTime = 0d;

                ProgBlockLCD.WriteText(PilotModeToString(), true);

                string speed = string.Format("Скорость: {0:#0.0} / ", VelocityVector.Length());
                if ((ForwardVectorNorm + ForwardVelocity).Length() > ForwardVelocity.Length())
                {
                    speed += string.Format("Вперед: {0:#0.0} / ", ForwardVelocity.Length());
                }
                else
                {
                    speed += string.Format("Назад:  {0:#0.0} / ", ForwardVelocity.Length());
                }
                if ((LeftVectorNorm + LeftVelocity).Length() > LeftVelocity.Length())
                {
                    speed += string.Format("Влево:  {0:#0.0}\n", LeftVelocity.Length());
                }
                else
                {
                    speed += string.Format("Вправо: {0:#0.0}\n", LeftVelocity.Length());
                }
                ProgBlockLCD.WriteText(speed, true);

                double sla;
                double sfa;
                Cockpit.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out sla);
                Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out sfa);
                ProgBlockLCD.WriteText(string.Format("Высота: пар. {0:#0.0} / ур. м. {1:#0.0} / пов. {2:#0.0}\n", altitude, sla, sfa), true);

                float thrust = 0;
                foreach (IMyThrust hover in HoverList)
                {
                    thrust += hover.MaxThrust;
                }
                float curmass = Cockpit.CalculateShipMass().PhysicalMass;
                float maxmass = thrust / (float)Cockpit.GetNaturalGravity().Length() * 3f;
                ProgBlockLCD.WriteText(string.Format("Груз: {0:# ### ###} кг из {1:# ### ###} кг / {2: ##.# %}\n", curmass, maxmass, curmass / maxmass), true);

                float        curvol = 0;
                float        maxvol = 0;
                IMyInventory inv;
                foreach (IMyCargoContainer cargo in CargoList)
                {
                    inv     = cargo.GetInventory();
                    curvol += (float)inv.CurrentVolume;
                    maxvol += (float)inv.MaxVolume;
                }
                ProgBlockLCD.WriteText(string.Format("Трюм: {0:# ### ###} л из {1:# ### ###} л / {2: ##.# %}\n", curvol * 1000, maxvol * 1000, curvol / maxvol), true);

                if (LCD != null)
                {
                    string t = ProgBlockLCD.GetText();
                    LCD.WriteText(t);
                }
            }
            runTime += Runtime.LastRunTimeMs;
        }