```
//这是一个自动使用摄像头扫描，并锁定目标，然后自动导航到目标附近的例子
Ship.Target LockTarget;
Ship MyShip;
void Main(string arguments)
{
//MyShip默认为null，如果为null或者执行new动作后Debug的值不是Normal，表示初始化失败缺少必要方块。则输出Debug并且跳出
if(MyShip == null || MyShip.Debug != "Normal"){
List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
GridTerminalSystem.GetBlocksOfType(blocks);
MyShip = new Ship(blocks);
Echo(MyShip.Debug);
return;
}
Ship.timetick ++ ; //更新Ship类的时间变量
MyShip.MotionInit();//还原所有推进器和陀螺仪的越级，并保持方块开启。
MyShip.UpdatePhysical();//更新时间及飞船坐标、运动状态等物理信息
//如果没有实例化锁定的目标 或 锁定目标已经实例化了，但EntityId是0
if(LockTarget == null || LockTarget.EntityId == 0){
//执行搜索前方1000米
Ship.Target target = MyShip.ScanPoint(MyShip.Cockpit.GetPosition() + MyShip.Cockpit.WorldMatrix.Forward*1000);
//如果目标不为空
if(!target.IsEmpty()){
//储存目标
LockTarget = target;
}
}else{
//如果存在已锁定的目标
//使用所有角度正确的摄像头自动追踪目标
Ship.Target target = MyShip.TrackTarget(LockTarget);
//如果追踪到目标，且与锁定的目标EntityId一致
if(!target.IsEmpty() && target.EntityId == LockTarget.EntityId){
//更新目标信息
LockTarget.UpdatePhysical(target);
//飞向目标
if(Vector3D.Distance(MyShip.Position, LockTarget.Position) >= 20){
MyShip.NavigationTo(LockTarget.Position);
}
}
//如果没追踪到目标，而且距离上次更新目标已经过去2秒
else if(Ship.timetick - LockTarget.TimeStamp >= 120){
//清空目标，切换到无目标状态
LockTarget = null;
}
}
//在编程块右下角显示帧计数变量
Echo(Ship.timetick.ToString());
}
// ==== 调起保存后自动运行 ====
Program(){
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
public class Ship
{
// ----- 静态属性 -----
public static int timetick; //帧计时器。注意：这是一个静态变量，使用Ship.timetick访问它，它是这个类共有的变量而不是某个实例化后的飞船特有的。
// ----- 方块类变量 -----
public IMyShipController Cockpit;
public IMyShipConnector Connector;
public IMyShipConnector MotherConnector;
public List<IMyLargeTurretBase> AutoWeapons = new List<IMyLargeTurretBase>();
public List<IMySmallGatlingGun> GatlingGuns = new List<IMySmallGatlingGun>();
public List<IMySmallMissileLauncher> RocketLaunchers = new List<IMySmallMissileLauncher>();
public List<IMyCameraBlock> Cameras = new List<IMyCameraBlock>();
public List<IMyGyro> Gyroscopes = new List<IMyGyro>();
public List<IMyThrust> Thrusts = new List<IMyThrust>();
public List<string> ThrustField = new List<string>();
public List<string> gyroYawField = new List<string>();
public List<string> gyroPitchField = new List<string>();
public List<string> gyroRollField = new List<string>();
public List<float> gyroYawFactor = new List<float>();
public List<float> gyroPitchFactor = new List<float>();
public List<float> gyroRollFactor = new List<float>();
// ---- 必要配置变量 ----
public string Debug = "Normal"; //错误报告，通过这个变量判断是否初始化成功
// ----- 运动信息和相关变量 -----
public Vector3D Position;
public Vector3D Velocity;
public Vector3D Acceleration;
public double Diameter;
// ---- 瞄准PID算法参数 ------
public double AimRatio = 3; //瞄准精度，单位：度。用来是否瞄准，以便其他动作判断。不影响瞄准的效率。当瞄准块的正前方向量与瞄准块和目标的连线向量夹角小于这个值时，整个系统判定瞄准了目标。
public int AimPID_T = 5; //PID 采样周期（单位：帧），周期越小效果越好，但太小的周期会让积分系数难以发挥效果
public double AimPID_P = 0.8; //比例系数：可以理解为整个PID控制的总力度，建议范围0到1.2，1是完全出力。
public double AimPID_I = 3; //积分系数：增加这个系数会让静态误差增加（即高速环绕误差），但会减少瞄准的震荡。反之同理
public double AimPID_D = 10; //微分系数：增加这个系数会减少瞄准的震荡幅度，但会加剧在小角度偏差时的震荡幅度。反之同理
// ----- 构造方法 ------
// 传入方块后自动处理方块并实例化
public Ship(){
this.Debug = "Empty Init Ship";
}
public Ship(List<IMyTerminalBlock> Blocks)
{
//这里面可以写入更详细的判断方块是否需要获取的条件，例如名字匹配
foreach(IMyTerminalBlock b in Blocks){
if(b is IMyShipController){
this.Cockpit = b as IMyShipController;
}
if(b is IMyCameraBlock){
this.Cameras.Add(b as IMyCameraBlock);
}
if(b is IMyLargeTurretBase){
this.AutoWeapons.Add(b as IMyLargeTurretBase);
}
if(b is IMySmallGatlingGun){
this.GatlingGuns.Add(b as IMySmallGatlingGun);
}
if(b is IMySmallMissileLauncher){
this.RocketLaunchers.Add(b as IMySmallMissileLauncher);
}
if(b is IMyGyro){
this.Gyroscopes.Add(b as IMyGyro);
}
if(b is IMyThrust){
this.Thrusts.Add(b as IMyThrust);
}
if(b is IMyShipConnector && (b as IMyShipConnector).OtherConnector != null){
this.Connector = b as IMyShipConnector;
this.MotherConnector = Connector.OtherConnector;
}
}
if(Cockpit == null) {Debug = "Cockpit Not Found"; return;}
Cameras.ForEach(delegate(IMyCameraBlock cam){cam.ApplyAction("OnOff_On");cam.EnableRaycast = true;});
//处理推进器
for(int i = 0; i < Thrusts.Count; i ++)
{
Base6Directions.Direction CockpitForward = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);
Base6Directions.Direction CockpitUp = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);
Base6Directions.Direction CockpitLeft = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);
switch (CockpitForward)
{ case Base6Directions.Direction.Forward: ThrustField.Add("Forward"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Backward"); break; }
switch (CockpitUp)
{ case Base6Directions.Direction.Forward: ThrustField.Add("Up"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Down"); break; }
switch (CockpitLeft)
{ case Base6Directions.Direction.Forward: ThrustField.Add("Left"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Right"); break; }
Thrusts[i].ApplyAction("OnOff_On");
}
//处理陀螺仪
for (int i = 0; i < Gyroscopes.Count; i++)
{
Base6Directions.Direction gyroUp = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);
Base6Directions.Direction gyroLeft = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);
Base6Directions.Direction gyroForward = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);
switch (gyroUp)
{ case Base6Directions.Direction.Up: gyroYawField.Add("Yaw"); gyroYawFactor.Add(1f); break;
case Base6Directions.Direction.Down: gyroYawField.Add("Yaw"); gyroYawFactor.Add(-1f); break;
case Base6Directions.Direction.Left: gyroYawField.Add("Pitch"); gyroYawFactor.Add(1f); break;
case Base6Directions.Direction.Right: gyroYawField.Add("Pitch"); gyroYawFactor.Add(-1f); break;
case Base6Directions.Direction.Forward: gyroYawField.Add("Roll"); gyroYawFactor.Add(-1f); break;
case Base6Directions.Direction.Backward: gyroYawField.Add("Roll"); gyroYawFactor.Add(1f); break;
}
switch (gyroLeft)
{ case Base6Directions.Direction.Up: gyroPitchField.Add("Yaw"); gyroPitchFactor.Add(1f); break;
case Base6Directions.Direction.Down: gyroPitchField.Add("Yaw"); gyroPitchFactor.Add(-1f); break;
case Base6Directions.Direction.Left: gyroPitchField.Add("Pitch"); gyroPitchFactor.Add(1f); break;
case Base6Directions.Direction.Right: gyroPitchField.Add("Pitch"); gyroPitchFactor.Add(-1f); break;
case Base6Directions.Direction.Forward: gyroPitchField.Add("Roll"); gyroPitchFactor.Add(-1f); break;
case Base6Directions.Direction.Backward: gyroPitchField.Add("Roll"); gyroPitchFactor.Add(1f); break;
}
switch (gyroForward)
{ case Base6Directions.Direction.Up: gyroRollField.Add("Yaw"); gyroRollFactor.Add(1f); break;
case Base6Directions.Direction.Down: gyroRollField.Add("Yaw"); gyroRollFactor.Add(-1f); break;
case Base6Directions.Direction.Left: gyroRollField.Add("Pitch"); gyroRollFactor.Add(1f); break;
case Base6Directions.Direction.Right: gyroRollField.Add("Pitch"); gyroRollFactor.Add(-1f); break;
case Base6Directions.Direction.Forward: gyroRollField.Add("Roll"); gyroRollFactor.Add(-1f); break;
case Base6Directions.Direction.Backward: gyroRollField.Add("Roll"); gyroRollFactor.Add(1f); break;
}
Gyroscopes[i].ApplyAction("OnOff_On");
}
}
/* ===== 摄像头扫描相关 ===== */
// ----- 摄像头搜索某个坐标点 -----
// 持续搜索座标
private int SP_Now_i;
public Target ScanPoint(Vector3D Point){
MyDetectedEntityInfo FoundTarget = new MyDetectedEntityInfo();
List<IMyCameraBlock> RightAngleCameras = Ship.GetCanScanCameras(this.Cameras, Point);
if(SP_Now_i >= RightAngleCameras.Count){SP_Now_i=0;}
double ScanSpeed = (RightAngleCameras.Count*2000)/(Vector3D.Distance(Point, this.Position)*60);//每个循环可用于扫描的摄像头个数
if(ScanSpeed >= 1)//每循环可扫描多个
{
FoundTarget = RightAngleCameras[SP_Now_i].Raycast(Point);
SP_Now_i += 1;
if(SP_Now_i >= RightAngleCameras.Count){SP_Now_i=0;}
}
else
{
if(Ship.timetick%Math.Round(1/ScanSpeed,0)==0)
{
FoundTarget = RightAngleCameras[SP_Now_i].Raycast(Point);
SP_Now_i += 1;
}
}
return new Target(FoundTarget);
}
// ----- 摄像头对多个坐标点执行脉冲扫描 -----
// 返回第一个扫描到的目标后跳出
// 可选传入一个EntityId作为匹配项
public Target PulseScanSingle(List<Vector3D> Points, long EntityId = 0){
MyDetectedEntityInfo FoundTarget = new MyDetectedEntityInfo();
int x = 0;//这样做是为了减少不必要的运算量
foreach(Vector3D p in Points){
for(int i = x; i < this.Cameras.Count; i ++){
if(this.Cameras[i].CanScan(p)){
FoundTarget = this.Cameras[i].Raycast(p);
if(!FoundTarget.IsEmpty()){
if(EntityId == 0){
return new Target(FoundTarget);
}
else if(FoundTarget.EntityId == EntityId){
return new Target(FoundTarget);
}
}
x = i;
break;
}
}
}
return new Target(FoundTarget);
}
// ----- 摄像头对多个坐标进行完全脉冲扫描 -----
// 将瞬间执行完整的扫描并返回所有扫描到的目标 （运算量较大）
public List<Target> PulseScanMultiple(List<Vector3D> Points){
List<Target> Res = new List<Target>();
List<MyDetectedEntityInfo> FoundTargets = new List<MyDetectedEntityInfo>();
int x = 0;//这样做是为了减少不必要的运算量
foreach(Vector3D p in Points){
for(int i = x; i < this.Cameras.Count; i ++){
if(this.Cameras[i].CanScan(p)){
MyDetectedEntityInfo FoundTarget = this.Cameras[i].Raycast(p);
if(!FoundTarget.IsEmpty()){
FoundTargets.Add(FoundTarget);
}
x = i;
break;
}
}
}
foreach(MyDetectedEntityInfo t in FoundTargets){
Res.Add(new Target(t));
}
return Res;
}
// ----- 追踪给定目标 -----
// 可传入Target类或MyDetectedEntityInfo类
private int TT_Now_i;
public Vector3D TrackDeviationToTarget; //基于目标坐标系的偏移量，用来修正中心锁定的问题
public Target TrackTarget(Target Tgt){
MyDetectedEntityInfo FoundTarget = new MyDetectedEntityInfo();
Vector3D posmove = Vector3D.TransformNormal(this.TrackDeviationToTarget, Tgt.Orientation);
Vector3D lidarHitPoint = Tgt.Position + posmove + (Ship.timetick - Tgt.TimeStamp)*Tgt.Velocity/60; //这个碰撞点算法是最正确的
List<IMyCameraBlock> RightAngleCameras = Ship.GetCanScanCameras(this.Cameras, lidarHitPoint);//获取方向正确的摄像头数量
if(TT_Now_i >= RightAngleCameras.Count){TT_Now_i=0;}
//执行常规追踪
double ScanSpeed = (RightAngleCameras.Count*2000)/((Vector3D.Distance(lidarHitPoint, this.Position))*60);//每个循环可用于扫描的摄像头个数
if(ScanSpeed >= 1)
{
for(int i = 1; i < ScanSpeed; i ++){
FoundTarget = RightAngleCameras[TT_Now_i].Raycast(lidarHitPoint);
TT_Now_i += 1;
if(TT_Now_i >= RightAngleCameras.Count){TT_Now_i=0;}
if(!FoundTarget.IsEmpty() && FoundTarget.EntityId == Tgt.EntityId){
return new Target(FoundTarget);
}
}
}
else
{
//这里向上取整实际上是采用了更低一点的频率在扫描，有利于恢复储能
if(Ship.timetick%Math.Ceiling(1/ScanSpeed)==0)
{
FoundTarget = RightAngleCameras[TT_Now_i].Raycast(lidarHitPoint);
TT_Now_i += 1;
if(TT_Now_i >= RightAngleCameras.Count){TT_Now_i=0;}
if(!FoundTarget.IsEmpty() && FoundTarget.EntityId == Tgt.EntityId){
return new Target(FoundTarget);
}
//常规未找到，继续遍历摄像头进行搜索
if(FoundTarget.IsEmpty() || FoundTarget.EntityId != Tgt.EntityId){
for(int i = 0; i < RightAngleCameras.Count; i ++){
FoundTarget = RightAngleCameras[TT_Now_i].Raycast(lidarHitPoint);
TT_Now_i += 1;
if(TT_Now_i >= RightAngleCameras.Count){TT_Now_i=0;}
if(!FoundTarget.IsEmpty() && FoundTarget.EntityId == Tgt.EntityId){
return new Target(FoundTarget);
}
}
}
}
}
//遍历搜索也未找到，进行脉冲阵面扫描
if(FoundTarget.IsEmpty() || FoundTarget.EntityId != Tgt.EntityId){
if(ScanSpeed >= 1 || Ship.timetick%Math.Ceiling(1/ScanSpeed)==0){
int LostTick = Ship.timetick - Tgt.TimeStamp;
double S_Radius = Tgt.Diameter*1.5; //搜索半径为目标直径1.5倍
double S_Interval = Tgt.Diameter/5; //搜索间隙是目标直径的1/5
Vector3D CenterPoint = Tgt.Position + Tgt.Velocity*LostTick/60 + Vector3D.Normalize(Tgt.Position - this.Position)*Tgt.Diameter/2;
List<Vector3D> Points = new List<Vector3D>();
Points.Add(CenterPoint);
//这里计算出与飞船和目标连线垂直，且互相垂直的两个向量，用作x和y方向遍历
Vector3D Vertical_X = Ship.CaculateVerticalVector((CenterPoint - this.Position), CenterPoint);
Vector3D Vertical_Y = Vector3D.Normalize(Vector3D.Cross((CenterPoint - this.Position), Vertical_X));
for(double x = 0; x < S_Radius; x += S_Interval){
for(double y = 0; y < S_Radius; y += S_Interval){
Points.Add(CenterPoint + Vertical_X*x + Vertical_Y*y);
Points.Add(CenterPoint + Vertical_X*(-x) + Vertical_Y*y);
Points.Add(CenterPoint + Vertical_X*x + Vertical_Y*(-y));
Points.Add(CenterPoint + Vertical_X*(-x) + Vertical_Y*(-y));
}
}
FoundTarget = this.PulseScanSingle(Points, Tgt.EntityId).EntityInfo;
if(!FoundTarget.IsEmpty() && FoundTarget.EntityId == Tgt.EntityId){
MatrixD TargetMainMatrix = FoundTarget.Orientation;
MatrixD TargetLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), TargetMainMatrix.Forward, TargetMainMatrix.Up);
Vector3D hitpoint = new Vector3D();
Vector3D.TryParse(FoundTarget.HitPosition.ToString(), out hitpoint);
hitpoint = hitpoint + Vector3D.Normalize(hitpoint - this.Position)*2;
this.TrackDeviationToTarget = Vector3D.TransformNormal(hitpoint - FoundTarget.Position, TargetLookAtMatrix);
return new Target(FoundTarget);
}
}
}
return new Target(FoundTarget);
}
/* ===== 运动控制相关 ===== */
// ----- 运动方块还原 -----
public void MotionInit(){
this.SetThrustOverride("All",0);
this.SetGyroOverride(false);
}
// ----- 更新飞船物理信息 -----
// 会自动更新本飞船的基本物理状态信息和时钟变量timetick
public void UpdatePhysical(){
this.Diameter = (this.Cockpit.CubeGrid.Max - this.Cockpit.CubeGrid.Min).Length() * this.Cockpit.CubeGrid.GridSize;
this.Acceleration = ((this.Cockpit.GetPosition() - this.Position) * 60 - this.Velocity) * 60;
this.Velocity = (this.Cockpit.GetPosition() - this.Position) * 60;
this.Position = this.Cockpit.GetPosition();
}
// ----- 瞄准坐标点 -----
// 使用PID算法，控制陀螺仪瞄准
// 返回是否瞄准完成
private List<Vector3D> Aim_PID_Data = new List<Vector3D>();
public bool AimAtPosition(Vector3D TargetPos)
{
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), this.Cockpit.WorldMatrix.Forward, this.Cockpit.WorldMatrix.Up);
Vector3D PositionToMe = Vector3D.Normalize(Vector3D.TransformNormal(TargetPos - this.Position, refLookAtMatrix));
//储存采样点
if(Aim_PID_Data.Count < AimPID_T){
for(int i = 0; i < AimPID_T; i ++){
Aim_PID_Data.Add(new Vector3D());
}
}
else{Aim_PID_Data.Remove(Aim_PID_Data[0]); Aim_PID_Data.Add(PositionToMe);}
//获得采样点积分
double X_I = 0;
double Y_I = 0;
foreach(Vector3D datapoint in Aim_PID_Data){
X_I += datapoint.X;
Y_I += datapoint.Y;
}
//计算输出
double YawValue = AimPID_P*(PositionToMe.X + (1/AimPID_I)*X_I + AimPID_D*(Aim_PID_Data[AimPID_T-1].X - Aim_PID_Data[0].X)/AimPID_T) * 60;
double PitchValue = AimPID_P*(PositionToMe.Y + (1/AimPID_I)*Y_I + AimPID_D*(Aim_PID_Data[AimPID_T-1].Y - Aim_PID_Data[0].Y)/AimPID_T) * 60;
this.SetGyroValue(YawValue, PitchValue, 0);
this.SetGyroOverride(true);
// 计算当前与预期瞄准点的瞄准夹角
Vector3D V_A = TargetPos - this.Position;
Vector3D V_B = this.Cockpit.WorldMatrix.Forward;
double Angle = Math.Acos(Vector3D.Dot(V_A,V_B)/(V_A.Length() * V_B.Length())) * 180 / Math.PI;
if(Angle <= AimRatio){return true;}
else{return false;}
}
// ----- 导航到坐标点 -----
// 支持传入一个参考速度来跟踪目标进行速度匹配
// 返回是否导航完成
// 依赖SingleDirectionThrustControl()方法来执行对xyz三轴的独立运算，运算考虑了推进器是否可以工作，但不考虑供电带来的效率问题。
// 本方法的结果路径是一个加速-减速-停止路径，通常不会错过目标，在此前提下本方法时间最优，在减速阶段处于对向推进器频繁满载震荡状态，在物理结果上是匀减速运动。
public bool NavigationTo(Vector3D Pos, Vector3D TargetVelocity = new Vector3D())
{
double ShipMass = this.Cockpit.CalculateShipMass().PhysicalMass;
//这个ThrustsPowers经过计算后，分别代表前后左右上下6个方向的理论最大加速度
double[] ThrustsPowers = new double[6];
for(int i = 0; i < this.Thrusts.Count; i ++){
if(this.Thrusts[i].IsFunctional){
switch(this.ThrustField[i]){
case("Backward"): ThrustsPowers[0] += this.Thrusts[i].MaxEffectiveThrust; break;
case("Forward"): ThrustsPowers[1] += this.Thrusts[i].MaxEffectiveThrust; break;
case("Right"): ThrustsPowers[2] += this.Thrusts[i].MaxEffectiveThrust; break;
case("Left"): ThrustsPowers[3] += this.Thrusts[i].MaxEffectiveThrust; break;
case("Down"): ThrustsPowers[4] += this.Thrusts[i].MaxEffectiveThrust; break;
case("Up"): ThrustsPowers[5] += this.Thrusts[i].MaxEffectiveThrust; break;
}
}
}
for(int i = 0; i < ThrustsPowers.Length; i ++){
ThrustsPowers[i] /= ShipMass;
}
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), this.Cockpit.WorldMatrix.Forward, this.Cockpit.WorldMatrix.Up);
//这里的PosToMe表示目标坐标基于自己坐标系的座标，x左-右+，y下-上+，z前-后+，MeVelocityToMe使用相同的坐标系规则，表示自己的速度基于自己坐标系
Vector3D PosToMe = Vector3D.TransformNormal(Pos - this.Position, refLookAtMatrix);
Vector3D MeVelocityToMe = Vector3D.TransformNormal(this.Velocity, refLookAtMatrix);
this.SingleDirectionThrustControl(PosToMe.Z, MeVelocityToMe.Z, ThrustsPowers[0], ThrustsPowers[1], "Backward", "Forward", 0.5);
this.SingleDirectionThrustControl(PosToMe.X, MeVelocityToMe.X, ThrustsPowers[2], ThrustsPowers[3], "Right", "Left", 0.5);
this.SingleDirectionThrustControl(PosToMe.Y, MeVelocityToMe.Y, ThrustsPowers[5], ThrustsPowers[4], "Up", "Down", 0.5);
if(TargetVelocity.Length() != 0){
Vector3D TargetVelocityToMe = Vector3D.TransformNormal(TargetVelocity - this.Velocity, refLookAtMatrix);
if(TargetVelocityToMe.X > 0){SetThrustOverride("Left", 100);}else if(TargetVelocityToMe.X < 0){SetThrustOverride("Right", 100);}
if(TargetVelocityToMe.Y > 0){SetThrustOverride("Down", 100);}else if(TargetVelocityToMe.Y < 0){SetThrustOverride("Up", 100);}
if(TargetVelocityToMe.Z > 0){SetThrustOverride("Forward", 100);}else if(TargetVelocityToMe.X < 0){SetThrustOverride("Backward", 100);}
}
if(PosToMe.Length() <= 1){return true;}
return false;
}
// ----- 导航到坐标点（辅助方法） -----
// 用于辅助NavigationTo()方法
// 传入基于自己坐标系的目标单向方向，自己的单向速度，正向加速度，反向加速度，正向推进器方向名，反向推进器方向名，导航精度
public void SingleDirectionThrustControl(double PosToMe, double VelocityToMe, double PostiveMaxAcceleration, double NagtiveMaxAcceleration, string PostiveThrustDirection, string NagtiveThrustDirection, double StopRatio){
if(PosToMe < -StopRatio){
double StopTime = -VelocityToMe/NagtiveMaxAcceleration;
if(StopTime < 0){
this.SetThrustOverride(PostiveThrustDirection, 100);
this.SetThrustOverride(NagtiveThrustDirection, 0);
}
else{
double StopDistance = -VelocityToMe*StopTime + 0.5*NagtiveMaxAcceleration*StopTime*StopTime;
if(Math.Abs(PosToMe) > StopDistance){
this.SetThrustOverride(PostiveThrustDirection, 100);
this.SetThrustOverride(NagtiveThrustDirection, 0);
}
else{
this.SetThrustOverride(PostiveThrustDirection, 0);
this.SetThrustOverride(NagtiveThrustDirection, 100);
}
}
}
else if(PosToMe > StopRatio){
double StopTime = VelocityToMe/NagtiveMaxAcceleration;
if(StopTime < 0){
//此时目标在后，运动速度是向前的
this.SetThrustOverride(PostiveThrustDirection, 0);
this.SetThrustOverride(NagtiveThrustDirection, 100);
}
else{
double StopDistance = VelocityToMe*StopTime + 0.5*NagtiveMaxAcceleration*StopTime*StopTime;
if(Math.Abs(PosToMe) > StopDistance){
//实际距离大于刹车距离，执行推进
this.SetThrustOverride(PostiveThrustDirection, 0);
this.SetThrustOverride(NagtiveThrustDirection, 100);
}
else{
//实际距离小于刹车距离，执行刹车
this.SetThrustOverride(PostiveThrustDirection, 100);
this.SetThrustOverride(NagtiveThrustDirection, 0);
}
}
}
else{
this.SetThrustOverride(PostiveThrustDirection, 0);
this.SetThrustOverride(NagtiveThrustDirection, 0);
}
}
/* ===== 方块控制相关 ===== */
// ----- 基础控制类方法 -----
public void TurnBlocksOnOff(List<IMyTerminalBlock> B, bool o)
{foreach(var b in B){b.ApplyAction(o?"OnOff_On":"OnOff_Off");}}
public void TurnBlocksOnOff(List<IMyGyro> B, bool o)
{foreach(var b in B){b.ApplyAction(o?"OnOff_On":"OnOff_Off");}}
public void TurnBlocksOnOff(List<IMyThrust> B, bool o)
{foreach(var b in B){b.ApplyAction(o?"OnOff_On":"OnOff_Off");}}
public void TurnBlocksOnOff(List<IMyLargeTurretBase> B, bool o)
{foreach(var b in B){b.ApplyAction(o?"OnOff_On":"OnOff_Off");}}
// ----- 控制推进器越级 -----
// 可传入Direcation = "All"、"Backward"、"Forward"、"Left"、"Right"、"Up"、"Down"
public void SetThrustOverride(string Direction, double Value)
{
if(Value > 100){Value = 100;}
if(Value < 0){Value = 0;}
for(int i = 0; i < this.Thrusts.Count; i ++){
if(Direction == "All"){this.Thrusts[i].ThrustOverridePercentage = (float)Value;}
else{if(this.ThrustField[i] == Direction){this.Thrusts[i].ThrustOverridePercentage = (float)Value;}}
}
}
// ----- 开关陀螺仪越级 -----
public void SetGyroOverride(bool bOverride)
{foreach(IMyGyro g in this.Gyroscopes){g.GyroOverride = bOverride;}}
// ----- 控制陀螺仪越级 -----
// 传入基于主控Cockpit的Yaw Pitch Roll，自动检测所有陀螺仪执行对应控制
public void SetGyroValue(double Y, double P, double R)
{
for (int i = 0; i < this.Gyroscopes.Count; i++){
this.Gyroscopes[i].SetValue(gyroYawField[i], (float)Y * gyroYawFactor[i]);
this.Gyroscopes[i].SetValue(gyroPitchField[i], (float)P * gyroPitchFactor[i]);
this.Gyroscopes[i].SetValue(gyroRollField[i], (float)R * gyroRollFactor[i]);
}
}
// ----- 控制陀螺仪越级 -----
// 可传入基于主控Cockpit的Yaw、Pitch、Roll中的单个轴进行控制，而不影响其他轴的状态
public void SetGyroValue(string Field, double Value)
{
switch(Field){
case("Yaw"):
for (int i = 0; i < this.Gyroscopes.Count; i++){
this.Gyroscopes[i].SetValue(gyroYawField[i], (float)Value * gyroYawFactor[i]);
}
break;
case("Pitch"):
for (int i = 0; i < this.Gyroscopes.Count; i++){
this.Gyroscopes[i].SetValue(gyroPitchField[i], (float)Value * gyroPitchFactor[i]);
}
break;
case("Roll"):
for (int i = 0; i < this.Gyroscopes.Count; i++){
this.Gyroscopes[i].SetValue(gyroRollField[i], (float)Value * gyroRollFactor[i]);
}
break;
}
}
// ----- 让所有武器射击一次 -----
public void WeaponsShoot()
{
this.GatlingGuns.ForEach(delegate(IMySmallGatlingGun g){g.ApplyAction("ShootOnce");});
this.RocketLaunchers.ForEach(delegate(IMySmallMissileLauncher g){g.ApplyAction("ShootOnce");});
}
/* ===== Target类 ===== */
// 由于摄像头、探测器等获取到的目标是 MyDetectedEntityInfo 类型，不方便计算和更新
// 本类中统一将其转换为Target类进行操作
public class Target
{
public string Name; //名字
public long EntityId; //唯一ID，当这个值为0可判断该Target是空的
public double Diameter; //半径
public int TimeStamp; //记录时间戳、基于Ship类中的timetick的值来记录
public Vector3D Position;
public Vector3D Velocity;
public Vector3D Acceleration;
public Vector3D HitPosition;
public MatrixD Orientation;
public Vector3D AccurateLockPositionToTarget;
public MyDetectedEntityInfo EntityInfo;
public Target(){
this.EntityId = 0;
this.TimeStamp = 0;
this.EntityInfo = new MyDetectedEntityInfo();
}
public Target(MyDetectedEntityInfo thisEntity){
this.EntityId = thisEntity.EntityId;
this.Name = thisEntity.Name;
this.Diameter = Vector3D.Distance(thisEntity.BoundingBox.Max, thisEntity.BoundingBox.Min)/2;
Vector3D.TryParse(thisEntity.Position.ToString(), out this.Position);
Vector3D.TryParse(thisEntity.Velocity.ToString(), out this.Velocity);
Vector3D.TryParse(thisEntity.HitPosition.ToString(), out this.HitPosition);
this.Acceleration = new Vector3D();
this.Orientation = thisEntity.Orientation;
this.TimeStamp = Ship.timetick;
this.EntityInfo = thisEntity;
}
public void UpdatePhysical(MyDetectedEntityInfo NewInfo){
this.Diameter = Vector3D.Distance(NewInfo.BoundingBox.Max, NewInfo.BoundingBox.Min)/2;
Vector3D.TryParse(NewInfo.Position.ToString(), out this.Position);
Vector3D.TryParse(NewInfo.HitPosition.ToString(), out this.HitPosition);
Vector3D velocity = new Vector3D();
Vector3D.TryParse(NewInfo.Velocity.ToString(), out velocity);
this.Acceleration = (velocity - this.Velocity)*60/(Ship.timetick - this.TimeStamp > 0 ? Ship.timetick - this.TimeStamp : 1);
this.Velocity = velocity;
this.Orientation = NewInfo.Orientation;
this.TimeStamp = Ship.timetick;
this.EntityInfo = NewInfo;
}
public void UpdatePhysical(Target _T){
this.UpdatePhysical(_T.EntityInfo);
}
public bool IsEmpty(){
return this.EntityId == 0;
}
}
/* ===== 静态方法 ===== */
// 静态方法通常是一些纯计算的方法，静态方法属于Ship这个类而不是某个实例化出来的Ship对象。
// 使用静态方法时，请直接使用 Ship.GetCanScanCameras()，而不是对实例化出来的某个Ship实体使用。
// -- 按FCS 8.0通讯标准读取一条参数 --
static string GetOneInfo(string CustomData, string Title, string ArgName){
string[] infos = CustomData.Split('\n');
bool right = false;
for(int i = 0; i < infos.Length; i ++){
if(infos[i].Contains("[" + Title + "]")){
right = true;
}
if(right && infos[i].Split('=')[0] == ArgName){
return infos[i].Split('=')[1];
}
if(infos[i].Contains("[\\" + Title + "]")){
break;
}
}
return "";
}
// -- 计算子弹碰撞点 --
public static Vector3D HitPointCaculate(Vector3D Me_Position, Vector3D Me_Velocity, Vector3D Me_Acceleration, Vector3D Target_Position, Vector3D Target_Velocity, Vector3D Target_Acceleration,
double Bullet_InitialSpeed, double Bullet_Acceleration, double Bullet_MaxSpeed)
{
//迭代算法
Vector3D HitPoint = new Vector3D();
Vector3D Smt = Target_Position - Me_Position;//发射点指向目标的矢量
Vector3D Velocity = Target_Velocity - Me_Velocity; //目标飞船和自己飞船总速度
Vector3D Acceleration = Target_Acceleration; //目标飞船和自己飞船总加速度
double AccTime = (Bullet_Acceleration == 0 ? 0 : (Bullet_MaxSpeed - Bullet_InitialSpeed)/Bullet_Acceleration);//子弹加速到最大速度所需时间
double AccDistance = Bullet_InitialSpeed*AccTime + 0.5*Bullet_Acceleration*AccTime*AccTime;//子弹加速到最大速度经过的路程
double HitTime = 0;
if(AccDistance < Smt.Length())//目标在炮弹加速过程外
{
HitTime = (Smt.Length() - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;
HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;
}
else//目标在炮弹加速过程内
{
double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;
double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;
HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);
HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;
}
//迭代，仅迭代更新碰撞时间，每次迭代更新右5位数量级
for(int i = 0; i < 3; i ++)
{
if(AccDistance < Vector3D.Distance(HitPoint,Me_Position))//目标在炮弹加速过程外
{
HitTime = (Vector3D.Distance(HitPoint,Me_Position) - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;
HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;
}
else//目标在炮弹加速过程内
{
double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;
double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;
HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);
HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;
}
}
return HitPoint;
}
// -- 计算可扫描的摄像头 --
public static List<IMyCameraBlock> GetCanScanCameras(List<IMyCameraBlock> Cams, Vector3D Point)
{
List<IMyCameraBlock> res = new List<IMyCameraBlock>();
foreach(IMyCameraBlock cm in Cams){
if(cm.IsFunctional && cm.CanScan(Point)){
res.Add(cm);
}
}
return res;
}
// -- 计算某向量的垂直向量 --
// 传入一个向量，和一个点，返回沿这个点出发与传入向量垂直的归一化向量
public static Vector3D CaculateVerticalVector(Vector3D Vector, Vector3D Point)
{
double x = 1;
double y = 1;
double z = (Point.X*Vector.X + Point.Y*Vector.Y + Point.Z*Vector.Z)/Vector.Z;
return Vector3D.Normalize(new Vector3D(x,y,z));
}
}
```
- 序言
- 写在前面的话
- 太空工程师
- MEA小组
- 一、入门
- 1.1 基础概念
- 1.2 编程工具
- 1.3 变量
- 1.4 函数 Function
- 1.5 基本语法
- 1.5.1 运算符
- 1.5.2 if
- 1.5.3 for
- 1.5.4 其他语法
- 1.3 类 Class
- 二、编程块
- 2.1 方块的概念
- 2.2 List<T>结构
- 2.3 获取方块
- 2.4 方块的使用
- 三、Ship 类
- 3.1 简介
- Ship v0.5
- 代码
- 手册（待更新）
- 例子（待更新）
- Ship v1.0
- 代码
- 例子
- 文档
- 实例化
- 内置变量
- 内置方法
- Target类
- 四、运动控制算法在SE中的应用
- 4.1 运动控制介绍
- 4.2 过程控制
- 4.3 震荡和动态误差
- 4.4 误差累加方案
- 4.5 PID算法
- 4.6 对PID算法的一点点简化
- 4.7 一阶惯性系统的PID算法优化的研究
- 五、MEA方块类
- 5.0 核心代码目录
- v1.0核心代码
- v1.1 核心代码
- v2.0 核心代码
- 5.1 类的概念
- 5.2 MEA的方块类（Block）
- 5.3 方块类文档
- 5.4 方块类2.0 全教程
- 5.4.1 安装和使用
- 5.4.2 方块类（Block）
- 5.4.3 显示屏类（DisplayScreen）
- 5.4.4 LCD类（LCD）
- 5.4.5 主控座椅类（Cockpit）
- 六、疯猴的编程笔记
- 第一个程序
- 获取和控制其他块
- 物流与生产
- 界面与通信
- 运动与姿态
- 侦测与导航
- 七、SteamZhou的笔记
- 有趣而花里胡哨的IDEA
- 八、质子对撞炮的笔记
- 属性 Property
- 接口 interface