//By dudujerry
//2021.4.17
//自动导航 v1.0 beta
Vector3D TARGET = new Vector3D(158267.95,-25453.61,-60794.93);
List<IMyThrust> _thrusts = new List<IMyThrust>();
List<IMyShipController> _shipControls = new List<IMyShipController>();
List<IMyCameraBlock> _camera = new List<IMyCameraBlock>();
List<IMyGyro> _gyros = new List<IMyGyro>();
IMyShipController _cockpit;
Vector3D _MaxAngleAcceleration;
Vector3D _InitAngleVelocity;
Vector3D targetThrustPower = new Vector3D();
Vector3D t = new Vector3D();
bool _dam;
int debug = 0;
double debug2 = 0;
int _state;//0代表正在寻找位置,1代表降落
double _maxThrust;
float _height;
bool _flag = false;
float bac = 0,fr = 0,lef = 0 ,rig = 0,up = 0,down = 0;
const string COCKPIT_NAME = "MainCockpit";
const double MAX_DISTANCE = 2000;
const double GYRO_I = 30;
const double GYRO_D = 30;
const double MAX_ERROR = 0.1;
const double MAX_ANGLE_ERROR = 0.5;
const double MAX_VELOCITY_ERROR = 0.5;
const double GYRO_MULTEXTRA = 2;
const int GYRO_T = 30;
const float _ELE_MAX_ERROR = 2;
const float _MIN_VELOCITY = 2;
const float MAX_VELOCITY = 100;
const double THRUST_I = 30;
const double THRUST_D = 30;
const int THRUST_T = 30;
const double THRUST_MULTEXTRA = 1;
const double _extra = 100;
class PIDClass{
public double getPID(double d){
d_data.Add(d);
if(d_data.Count == 1){
d_data.Add(d);
}
if(d_data.Count > T){
d_data.Remove(d_data[0]);
}
double sum_d = 0;
foreach(double i in d_data){
sum_d += i;
}
double fir = d + 1/I*(sum_d/d_data.Count) + D*(d_data[d_data.Count - 1] - d_data[d_data.Count - 2]);
if(P2 == 0 || (fir >= 0 && P2 > 0))
return P * fir; //输出结果
else if(fir < 0 && P2 > 0){
return P2*fir;
}
return 0;
}
public void setMultiplier(int t,double p,double i,double d){
T = t;
P = p;
I = i;
D = d;
}
public void setExtraMultiplier(double p2){
P2 = p2;
}
public PIDClass(int t,double p,double i,double d){
T = t;
P = p;
I = i;
D = d;
}
private int T = 30; //周期，这个周期不是公式中的时间间隔T，而是储存累加结果的周期
private double P = 0.0001; //比例系数
private double I = 10; //积分系数
private double D = 50; //微分系数
private double P2 = 0;
private List<double> d_data = new List<double>();
};
PIDClass gyroPIDX,gyroPIDY,gyroPIDZ;
PIDClass thrustPIDX,thrustPIDY,thrustPIDZ;
PIDClass thrustPIDY2,thrustPIDY3,thrustPIDZ2;
PIDClass VthrustPIDX,VthrustPIDY,VthrustPIDZ;
PIDClass VthrustPIDY2,VthrustPIDY3,VthrustPIDZ2;
PIDClass VthrustPIDX4,VthrustPIDY4,VthrustPIDZ4;
void PrintVector3D(Vector3D v){
Echo("X="+Math.Round(v.X,4).ToString());
Echo("Y="+Math.Round(v.Y,4).ToString());
Echo("Z="+Math.Round(v.Z,4).ToString());
}
public void AddVector(Vector3D addvector,IMyShipController cockpit,List<IMyThrust> thrusts){
float ShipMass = cockpit.CalculateShipMass().PhysicalMass;
//MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(),cockpit.WorldMatrix.Forward, cockpit.WorldMatrix.Up);
//addvector = Vector3D.TransformNormal(addvector,refLookAtMatrix);
addvector = addvector*ShipMass;
double totalG = addvector.Length() ;
double totalthrust = 0;//debug
foreach(var thrust in thrusts){
thrust.ThrustOverride = 0;
thrust.ThrustOverridePercentage = 0;
var maxthrust = thrust.MaxEffectiveThrust;
thrust.Enabled = false;
if(!thrust.IsFunctional){
continue;
}
switch (cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward))
{
//设定推力
case Base6Directions.Direction.Backward:
thrust.ThrustOverridePercentage=(targetThrustPower.Z>0)?(float)targetThrustPower.Z:0;
thrust.Enabled = (targetThrustPower.Z>0)?true:false;
if(addvector.Z > 0 && !_dam ){
thrust.Enabled = true;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <addvector.Z + thrust.ThrustOverride){
thrust.ThrustOverride = maxthrust;
addvector.Z -= maxthrust - oldth;
}
else{
thrust.ThrustOverride += (float)addvector.Z;
addvector.Z = 0;
}
}
break;
case Base6Directions.Direction.Forward:
thrust.ThrustOverridePercentage=(targetThrustPower.Z<0)?(float)-targetThrustPower.Z:0;
thrust.Enabled = (targetThrustPower.Z<0)?true:false;
if(addvector.Z < 0&& !_dam){
thrust.Enabled = true;
float fz = (-1) * (float)addvector.Z;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <fz + oldth){
thrust.ThrustOverride = maxthrust;
addvector.Z += maxthrust - oldth;
}
else{
thrust.ThrustOverride += fz;
addvector.Z = 0;
}
}
break;
case Base6Directions.Direction.Right:
thrust.ThrustOverridePercentage=(targetThrustPower.X>0)?(float)targetThrustPower.X:0;
thrust.Enabled = (targetThrustPower.X>0)?true:false;
if(addvector.X > 0&& !_dam){
thrust.Enabled = true;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <addvector.X + oldth){
thrust.ThrustOverride = maxthrust;
addvector.X -= maxthrust - oldth;
}
else{
thrust.ThrustOverride += (float)addvector.X;
addvector.X = 0;
}
}
break;
case Base6Directions.Direction.Left:
thrust.ThrustOverridePercentage=(targetThrustPower.X<0)?(float)-targetThrustPower.X:0;
thrust.Enabled = (targetThrustPower.X<0)?true:false;
if(addvector.X < 0&& !_dam){
thrust.Enabled = true;
float fz = (-1) * (float)addvector.X;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <fz + oldth){
thrust.ThrustOverride = maxthrust;
addvector.X += maxthrust - oldth;
}
else{
thrust.ThrustOverride += fz;
addvector.X = 0;
}
}
break;
case Base6Directions.Direction.Up:
thrust.ThrustOverridePercentage=(targetThrustPower.Y>0)?(float)targetThrustPower.Y:0;
thrust.Enabled = (targetThrustPower.Y>0)?true:false;
if(addvector.Y > 0&& !_dam){
thrust.Enabled = true;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <addvector.Y + oldth){
thrust.ThrustOverride = maxthrust;
addvector.Y -= maxthrust - oldth;
}
else{
thrust.ThrustOverride += (float)addvector.Y;
addvector.Y = 0;
int th = (int)thrust.ThrustOverride;
}
}
break;
case Base6Directions.Direction.Down:
thrust.ThrustOverridePercentage=(targetThrustPower.Y<0)?(float)-targetThrustPower.Y:0;
thrust.Enabled = (targetThrustPower.Y<0)?true:false;
if(addvector.Y < 0&& !_dam){
thrust.Enabled = true;
float fz = (-1) * (float)addvector.Y;
float oldth = (float)thrust.ThrustOverride;
if(maxthrust <fz + oldth){
thrust.ThrustOverride = maxthrust;
addvector.Y += maxthrust - oldth;
}
else{
thrust.ThrustOverride += fz;
addvector.Y = 0;
}
}
break;
}
totalthrust += thrust.ThrustOverride;//debug
}
}
void SetGyros(double pitch,double yaw,double roll){
foreach(var gyro in _gyros){
gyro.Enabled = true;
gyro.GyroOverride = true;
//gyro.Roll = (float)roll;
//gyro.Pitch = (float)pitch;
//gyro.Yaw = (float)yaw;
if(pitch == 0 && yaw == 0 && roll == 0)
gyro.GyroOverride = false;
switch (_cockpit.WorldMatrix.GetClosestDirection(gyro.WorldMatrix.Backward))
{//正右负左，正上负下
case Base6Directions.Direction.Forward:
gyro.Pitch = (float)-pitch;
gyro.Yaw = (float)yaw;
gyro.Roll = (float)-roll;
break;
case Base6Directions.Direction.Backward:
gyro.Pitch = (float)pitch;
gyro.Yaw = (float)yaw;
gyro.Roll = (float)roll;
break;
case Base6Directions.Direction.Right:
gyro.Pitch = (float)roll;
gyro.Roll = (float)-pitch;
gyro.Yaw = (float)yaw;
break;
case Base6Directions.Direction.Left:
gyro.Pitch = (float)roll;
gyro.Roll = (float)-pitch;
gyro.Yaw = (float)yaw;
break;
case Base6Directions.Direction.Down:
gyro.Pitch = (float)pitch;
gyro.Roll = (float)-yaw;
gyro.Yaw = (float)roll;
break;
case Base6Directions.Direction.Up:
gyro.Roll = (float)yaw;
gyro.Pitch = (float)-pitch;
gyro.Yaw = (float)roll;
break;
}
}
}
bool InitGyros(){//测陀螺仪最大角加速度
if(_MaxAngleAcceleration.Length() != 0){
//若已经测过了，返回
return true;
}
Vector3D angularVelocity = _cockpit.GetShipVelocities().AngularVelocity;
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);
Vector3D meAngleVelocityToMe = Vector3D.TransformNormal(angularVelocity, refLookAtMatrix);
//(*)-
double meRollAngleVelocity = meAngleVelocityToMe.Z*180/Math.PI; //当前角速度
double mePitchAngleVelocity = meAngleVelocityToMe.X*180/Math.PI;
double meYawAngleVelocity = meAngleVelocityToMe.Y*180/Math.PI;
//得到当前帧的角速度(*)-
if(_InitAngleVelocity.Length() == 0){
_InitAngleVelocity = new Vector3D(mePitchAngleVelocity,meYawAngleVelocity,meRollAngleVelocity);
SetGyros(60,60,60);
}
else{
_MaxAngleAcceleration = new Vector3D((mePitchAngleVelocity - _InitAngleVelocity.X)*60,(meYawAngleVelocity - _InitAngleVelocity.Y)*60,(meRollAngleVelocity - _InitAngleVelocity.Z)*60);
//计算最大角加速度，由于一秒60帧所以乘60
//gys.SetOverride(false);
//gys.Yaw = gys.Pitch = 0;
//返回原状态(*)-
SetGyros(0,0,0);
double P = 60/(GYRO_T*(1/GYRO_I*180+GYRO_D*180))*GYRO_MULTEXTRA;
gyroPIDX.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);
gyroPIDY.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);
gyroPIDZ.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);
}
return false;
}
bool gyroOperation(Vector3D target){
Vector3D err = new Vector3D(target.X,target.Y,target.Z);
if(err.Length() <= MAX_ERROR)
return true;
Vector3D nowtarget = new Vector3D(gyroPIDX.getPID(target.X),-gyroPIDY.getPID(target.Y),gyroPIDZ.getPID(target.Z));
SetGyros(nowtarget.X,nowtarget.Y,nowtarget.Z);
return false;
}
public static double TargetAngleToMe(Vector3D targetToMe,string direction){
targetToMe = Vector3D.Normalize(targetToMe);
Vector3D zForward = new Vector3D(0,0,-1);
targetToMe.X *= -1;//SE中，前负后正，右负左正，上负下正
targetToMe.Y *= -1;
targetToMe.Z *= -1;
if(direction == "Yaw"){
Vector3D targetYawVector = new Vector3D(targetToMe.X,0,targetToMe.Z);
return Math.Atan2(targetToMe.X,targetToMe.Z) * 180/Math.PI;
}
else if (direction == "Pitch"){
Vector3D targetYawVector = new Vector3D(0,targetToMe.Y,targetToMe.Z);
return Math.Atan2(targetToMe.Y,targetToMe.Z) * 180/Math.PI;
}
else if(direction == "Roll"){
Vector3D targetRollVector = new Vector3D(targetToMe.X,targetToMe.Y,0);
return Math.Atan2(targetToMe.X,targetToMe.Y) * 180/Math.PI;
}
return 0;
}
Vector3D GetTarget(string str){
//解析坐标
return new Vector3D(0,0,0);
}
public Program()
{
// 构造函数，每次脚本运行时会被首先调用一次。用它来初始化脚本。
// 构造函数是可选项，
// 如不需要可以删除。
//
// 建议这里设定RuntimeInfo.UpdateFrequency，
// 这样脚本就不需要定时器方块也能自动运行了。
Runtime.UpdateFrequency = UpdateFrequency.Update1;
_MaxAngleAcceleration = new Vector3D();
_InitAngleVelocity = new Vector3D();
debug = 0;
gyroPIDX = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);
gyroPIDY = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);
gyroPIDZ = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);
GridTerminalSystem.GetBlocksOfType<IMyGyro>(_gyros);
GridTerminalSystem.GetBlocksOfType<IMyShipController>(_shipControls);
GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(_camera);
GridTerminalSystem.GetBlocksOfType<IMyThrust>(_thrusts);
foreach(var cam in _camera){
cam.EnableRaycast = true;
}
bool flag = false;
foreach(var coc in _shipControls){
if(coc.CustomName == COCKPIT_NAME){
_cockpit = coc;
flag = true;
}
}
if(!flag) _cockpit = _shipControls[0];
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);
SetTarget();
/*if(_cockpit.GetNaturalGravity().Length() != 0){
Vector3D v3d = new Vector3D();
_cockpit.TryGetPlanetPosition(out v3d);
Vector3D tar_p = Vector3D.Normalize(t - v3d);
Vector3D me_p = Vector3D.Normalize(_cockpit.CenterOfMass - v3d);
double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);
v3d = Vector3D.TransformNormal(t - v3d,refLookAtMatrix);
_height = (float)v3d.Length();
double ele = 0;
_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);
}*/
foreach(var thrust in _thrusts){
switch (_cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward))
{
case Base6Directions.Direction.Backward:
bac += thrust.MaxEffectiveThrust;
break;
case Base6Directions.Direction.Forward:
fr += thrust.MaxEffectiveThrust;
break;
case Base6Directions.Direction.Left:
lef += thrust.MaxEffectiveThrust;
break;
case Base6Directions.Direction.Right:
rig += thrust.MaxEffectiveThrust;
break;
case Base6Directions.Direction.Up:
up += thrust.MaxEffectiveThrust;
_maxThrust += thrust.MaxEffectiveThrust;
break;
case Base6Directions.Direction.Down:
down += thrust.MaxEffectiveThrust;
break;
}
}
bac /= _cockpit.CalculateShipMass().PhysicalMass;
fr /= _cockpit.CalculateShipMass().PhysicalMass;
lef /= _cockpit.CalculateShipMass().PhysicalMass;
rig /= _cockpit.CalculateShipMass().PhysicalMass;
up /= _cockpit.CalculateShipMass().PhysicalMass;
down /= _cockpit.CalculateShipMass().PhysicalMass;
Vector3D distance = t - _cockpit.CenterOfMass;
t = Vector3D.TransformNormal(distance, refLookAtMatrix);
if(distance.X < 0) distance.X = -distance.X;
if(distance.Y < 0) distance.Y = -distance.Y;
if(distance.Z < 0) distance.Z = -distance.Z;
thrustPIDX = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*lef/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
thrustPIDY = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*down/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
thrustPIDZ = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*bac/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
//debug2 = angle;
VthrustPIDX = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.X),THRUST_I,THRUST_D);
VthrustPIDY = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Y),THRUST_I,THRUST_D);
VthrustPIDZ = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Z),THRUST_I,THRUST_D);
VthrustPIDX4 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.X),THRUST_I,THRUST_D);
VthrustPIDY4 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Y),THRUST_I,THRUST_D);
VthrustPIDZ4 = new PIDClass(THRUST_T,10*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Z),THRUST_I,THRUST_D);
thrustPIDX.setExtraMultiplier(20*THRUST_MULTEXTRA*rig/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
thrustPIDY.setExtraMultiplier(20*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
thrustPIDZ.setExtraMultiplier(20*THRUST_MULTEXTRA*fr/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
_cockpit.DampenersOverride = false;
}
public void Save()
{
// 当程序需要保存时，会发出提醒。使用这种方法可以将状态保存
// 至内存或其他路径。此为备选项，
// 如果不需要，可以删除。
foreach(var thrust in _thrusts){
thrust.Enabled = true;
thrust.ThrustOverridePercentage = 0;
}
foreach(var gyro in _gyros){
gyro.Enabled = true;
gyro.GyroOverride = false;
}
}
void SetTarget()
{
t = TARGET;
}
float GetH(float v){
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);
float ShipMass = _cockpit.CalculateShipMass().PhysicalMass;
Vector3D g = _cockpit.GetNaturalGravity();
float totalg = (float)g.Length();
float upv = (float)((_maxThrust - ShipMass*totalg) / (ShipMass));
//Echo("upv" + upv.ToString());
Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
float vv = (float)V.Length();
float nowv = vv;
float t = vv/upv;
float x = (float)(vv*t - 0.5*upv*t*t);
float totalh = x;
Echo("x = " + x.ToString());
return totalh + (float)_extra;
}
public void Main(string argument, UpdateType updateSource)
{
/*foreach(var thrust in _thrusts){
thrust.Enabled = true;
thrust.ThrustOverridePercentage = 0;
}
foreach(var gyro in _gyros){
gyro.Enabled = true;
gyro.GyroOverride = false;
}
return;*/
Echo("State="+_state.ToString());
targetThrustPower = new Vector3D();
if(InitGyros() == false) return;
_dam = _cockpit.DampenersOverride;
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);
Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
SetTarget();
Vector3D g = _cockpit.GetNaturalGravity();
float totalg = (float)g.Length();
if(_dam == true) targetThrustPower=-Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
if(totalg == 0)
{
t = t - _cockpit.CenterOfMass;
t = Vector3D.TransformNormal(t, refLookAtMatrix);
//
Vector3D tar = new Vector3D(TargetAngleToMe(t,"Pitch"),TargetAngleToMe(t,"Yaw"),0);
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
if(!gyroOperation(tar)) return;
if(!_cockpit.DampenersOverride)targetThrustPower = new Vector3D(0,0,0);
foreach(var cam in _camera){
int timebeforeray = cam.TimeUntilScan(MAX_DISTANCE);
if(timebeforeray > 0)
continue;
MyDetectedEntityInfo info = cam.Raycast(MAX_DISTANCE,0,0);
if(!info.IsEmpty())
_cockpit.DampenersOverride = true;
}
if(!_cockpit.DampenersOverride){
//Vector3D a = new Vector3D(-thrustPIDX.getPID(t.X),thrustPIDY.getPID(t.Y),thrustPIDZ.getPID(t.Z));
double tarv = VthrustPIDZ4.getPID(-t.Z);
double tara = thrustPIDZ.getPID(tarv + V.Z);
Vector3D a = new Vector3D(0,0,-tara);
Echo("tarv = " + (tarv+V.Z).ToString());
Echo("tara = " + tara.ToString());
if(Math.Abs(Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Z) >= 99)
a = new Vector3D(0,0,0);
AddVector(a,_cockpit,_thrusts);
}
}
else{
if(_flag == false) {
_state = -1;
Vector3D v3d = new Vector3D();
_cockpit.TryGetPlanetPosition(out v3d);
Vector3D tar_p = Vector3D.Normalize(t - v3d);
Vector3D me_p = Vector3D.Normalize(_cockpit.CenterOfMass - v3d);
double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);
v3d = Vector3D.TransformNormal(_cockpit.CenterOfMass - v3d,refLookAtMatrix);
_height = (float)v3d.Length();
double ele = 0;
_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);
thrustPIDY2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
thrustPIDY3 = new PIDClass(THRUST_T,10*THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
//debug2 = THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*_height);
thrustPIDZ2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*bac/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);
VthrustPIDY2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*_height),THRUST_I,THRUST_D);
VthrustPIDY3 = new PIDClass(THRUST_T,30*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*ele),THRUST_I,THRUST_D);
//debug2 = THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*_height);
VthrustPIDZ2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*angle),THRUST_I,THRUST_D);
thrustPIDY2.setExtraMultiplier(20*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
thrustPIDY3.setExtraMultiplier(10*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
//debug2 = THRUST_MULTEXTRA*up/ele;
thrustPIDZ2.setExtraMultiplier(20*THRUST_MULTEXTRA*fr/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));
}
_flag = true;
if(_state == -1){
double v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Length();
if(v > MAX_VELOCITY_ERROR) _cockpit.DampenersOverride = true;
else {
_state = 0;
_cockpit.DampenersOverride = false;
}
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
}
else if(_state == 0){
Vector3D plantPosition;
_cockpit.TryGetPlanetPosition(out plantPosition);
Vector3D tar_p = t - plantPosition;
Vector3D me_p = _cockpit.CenterOfMass - plantPosition;
double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);
t = Vector3D.TransformNormal(t,refLookAtMatrix);
g = Vector3D.TransformNormal(g,refLookAtMatrix);
Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,TargetAngleToMe(t,"Yaw"),TargetAngleToMe(g,"Roll"));
AddVector(-g,_cockpit,_thrusts);
if(!gyroOperation(gv)) return;
Vector3D moveg = -g;
double verr = VthrustPIDZ2.getPID(angle)+V.Z;
moveg.Z = -thrustPIDZ2.getPID(verr);//解决，双重PID
Echo("angle_err = "+angle.ToString());
Echo("velocity_err = " + verr.ToString());
Echo("PID outcome = " + moveg.Z.ToString());
double z_v = V.Z;
if(Math.Abs(z_v) >= MAX_VELOCITY-1 && z_v * moveg.Z > 0) moveg.Z = 0;
plantPosition = Vector3D.TransformNormal(_cockpit.CenterOfMass-plantPosition,refLookAtMatrix);
double vverr = VthrustPIDY2.getPID(_height - plantPosition.Length())-V.Y;
double y2 = thrustPIDY2.getPID(vverr);//高了为负，待测试
Echo("height_err = " + (_height - plantPosition.Length()).ToString());
Echo("verr = " + vverr.ToString());
Echo("PID outcome = " + y2.ToString());
moveg.Y += y2;
moveg.Y += z_v*z_v/plantPosition.Length();//***向心力
AddVector(moveg,_cockpit,_thrusts);
if(angle <= MAX_ANGLE_ERROR) _state = 1;
}
else if(_state == 1){
double v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Length();
if(v > MAX_VELOCITY_ERROR) _cockpit.DampenersOverride = true;
else {
_state = 2;
_cockpit.DampenersOverride = false;
}
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
}
else if(_state == 2){
/* PID方法 *
double ele = 0;
_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);
g = Vector3D.TransformNormal(g,refLookAtMatrix);
Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,0,TargetAngleToMe(g,"Roll"));
gyroOperation(gv);
double verr = VthrustPIDY3.getPID(ele)-V.Y;
Vector3D moveg = new Vector3D(0,-thrustPIDY3.getPID(verr)-g.Y,0);//待测试***
Echo("ele err = " + ele.ToString());
Echo("tar v = " + (verr+V.Y).ToString());
Echo("PID outcome = " + (moveg.Y+g.Y).ToString());
double y_v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Y;
if(y_v > MAX_VELOCITY-1 && y_v*moveg.Y > 0) moveg.Y = 0;
AddVector(moveg,_cockpit,_thrusts);*/
double ele = 0;
_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);
float minh = GetH((float)_cockpit.GetShipSpeed());
g = Vector3D.TransformNormal(g,refLookAtMatrix);
g = Vector3D.Normalize(g);
Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,0,TargetAngleToMe(g,"Roll"));
gyroOperation(gv);
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
//Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);
if(ele <= _ELE_MAX_ERROR ){
_state = 3;
}
else if(ele <= minh && V.Y < 0 && -V.Y >= _MIN_VELOCITY){
targetThrustPower = -g;
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
}
}
else if(_state == 3){
AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);
g = Vector3D.TransformNormal(g,refLookAtMatrix);
g = Vector3D.Normalize(g);
Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,TargetAngleToMe(t,"Yaw"),TargetAngleToMe(g,"Roll"));
gyroOperation(gv);
}
}
}