public void Init(ShipControlCommons shipControl,
Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
{
ShipForward = shipForward;
ShipUp = shipUp;
ShipLeft = Base6Directions.GetLeft(ShipUp, ShipForward);
var small = shipControl.Reference.CubeGrid.GridSize == 0.5f;
yawPID.Kp = small ? SmallGyroKp : LargeGyroKp;
yawPID.Ti = small ? SmallGyroTi : LargeGyroTi;
yawPID.Td = small ? SmallGyroTd : LargeGyroTd;
pitchPID.Kp = small ? SmallGyroKp : LargeGyroKp;
pitchPID.Ti = small ? SmallGyroTi : LargeGyroTi;
pitchPID.Td = small ? SmallGyroTd : LargeGyroTd;
rollPID.Kp = small ? SmallGyroKp : LargeGyroKp;
rollPID.Ti = small ? SmallGyroTi : LargeGyroTi;
rollPID.Td = small ? SmallGyroTd : LargeGyroTd;
yawPID.Reset();
pitchPID.Reset();
rollPID.Reset();
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
{
ShipForward = shipForward;
ShipUp = shipUp;
ShipLeft = Base6Directions.GetLeft(ShipUp, ShipForward);
double maxVel = Math.PI / 4.0;
yawPID.Kp = AngleKp;
yawPID.Ti = AngleTi;
yawPID.Td = AngleTd;
yawPID.min = -maxVel;
yawPID.max = maxVel;
pitchPID.Kp = AngleKp;
pitchPID.Ti = AngleTi;
pitchPID.Td = AngleTd;
pitchPID.min = -maxVel;
pitchPID.max = maxVel;
rollPID.Kp = AngleKp / 2.0; // Don't ask
rollPID.Ti = AngleTi;
rollPID.Td = AngleTd;
rollPID.min = -maxVel;
rollPID.max = maxVel;
yawVPID.Kp = VelKp;
yawVPID.Ti = VelTi;
yawVPID.Td = VelTd;
yawVPID.min = -Math.PI;
yawVPID.max = Math.PI;
pitchVPID.Kp = VelKp;
pitchVPID.Ti = VelTi;
pitchVPID.Td = VelTd;
pitchVPID.min = -Math.PI;
pitchVPID.max = Math.PI;
rollVPID.Kp = VelKp / 2.0; // Don't ask
rollVPID.Ti = VelTi;
rollVPID.Td = VelTd;
rollVPID.min = -Math.PI;
rollVPID.max = Math.PI;
yawPID.Reset();
pitchPID.Reset();
rollPID.Reset();
yawVPID.Reset();
pitchVPID.Reset();
rollVPID.Reset();
}
public void Start(ZACommons commons, EventDriver eventDriver)
{
var shipControl = (ShipControlCommons)commons;
shipControl.Reset(gyroOverride: true, thrusterEnable: null);
forwardCruiser.Init(shipControl,
localForward: shipControl.ShipForward);
upCruiser.Init(shipControl,
localForward: shipControl.ShipUp);
leftCruiser.Init(shipControl,
localForward: Base6Directions.GetLeft(shipControl.ShipUp, shipControl.ShipForward));
eventDriver.Schedule(0, Run);
}
public GyroDetails(IMyGyro gyro, Base6Directions.Direction shipUp,
Base6Directions.Direction shipForward)
{
Gyro = gyro;
AxisDetails = new GyroAxisDetails[3];
var shipLeft = Base6Directions.GetLeft(shipUp, shipForward);
// Determine yaw axis
SetAxisDetails(gyro, Yaw, shipUp);
// Determine pitch axis
SetAxisDetails(gyro, Pitch, shipLeft);
// Determine roll axis
SetAxisDetails(gyro, Roll, shipForward);
}
//recommended but not necessary, third to call if used (gyro system is set up then)
//passing OrientBlock=null will use the grid as reference
public void SetRefBlock(IMyTerminalBlock OrientBlock,
Base6Directions.Direction DirForward = Forward,
Base6Directions.Direction DirUp = Up)
{
if (Base6Directions.GetAxis(DirForward) == Base6Directions.GetAxis(DirUp))
{
DirUp = Base6Directions.GetPerpendicular(DirForward);
}
if (OrientBlock == null)
{
}
else
{
Vector3 RotatedVector = Base6Directions.GetVector(DirForward);
Vector3.TransformNormal(ref RotatedVector, OrientBlock.Orientation, out RotatedVector);
DirForward = Base6Directions.GetDirection(ref RotatedVector);
RotatedVector = Base6Directions.GetVector(DirUp);
Vector3.TransformNormal(ref RotatedVector, OrientBlock.Orientation, out RotatedVector);
DirUp = Base6Directions.GetDirection(ref RotatedVector);
}
RefUp = DirUp;
RefForward = DirForward;
RefLeft = Base6Directions.GetLeft(RefUp, RefForward);
}
private void Orientate(IMyTerminalBlock ReferencePoint)
{ // Big thanks to Skleroz for this awesome stuff.
Vector3 V3For = Base6Directions.GetVector(ReferencePoint.Orientation.TransformDirection(Base6Directions.Direction.Forward));
Vector3 V3Up = Base6Directions.GetVector(ReferencePoint.Orientation.TransformDirection(Base6Directions.Direction.Up));
V3For.Normalize();
V3Up.Normalize();
Base6Directions.Direction B6DFor = Base6Directions.GetDirection(V3For);
Base6Directions.Direction B6DTop = Base6Directions.GetDirection(V3Up);
Base6Directions.Direction B6DLeft = Base6Directions.GetLeft(B6DTop, B6DFor);
Base6Directions.Direction GyroUp = Gyro.Orientation.TransformDirectionInverse(B6DTop);
Base6Directions.Direction GyroForward = Gyro.Orientation.TransformDirectionInverse(B6DFor);
Base6Directions.Direction GyroLeft = Gyro.Orientation.TransformDirectionInverse(B6DLeft);
switch (GyroUp)
{
case Base6Directions.Direction.Up:
strYaw = "Yaw";
intYaw = 1;
break;
case Base6Directions.Direction.Down:
strYaw = "Yaw";
intYaw = -1;
break;
case Base6Directions.Direction.Left:
strYaw = "Pitch";
intYaw = 1;
break;
case Base6Directions.Direction.Right:
strYaw = "Pitch";
intYaw = -1;
break;
case Base6Directions.Direction.Backward:
strYaw = "Roll";
intYaw = 1;
break;
case Base6Directions.Direction.Forward:
strYaw = "Roll";
intYaw = -1;
break;
}
switch (GyroLeft)
{
case Base6Directions.Direction.Up:
strPitch = "Yaw";
intPitch = 1;
break;
case Base6Directions.Direction.Down:
strPitch = "Yaw";
intPitch = -1;
break;
case Base6Directions.Direction.Left:
strPitch = "Pitch";
intPitch = 1;
break;
case Base6Directions.Direction.Right:
strPitch = "Pitch";
intPitch = -1;
break;
case Base6Directions.Direction.Backward:
strPitch = "Roll";
intPitch = 1;
break;
case Base6Directions.Direction.Forward:
strPitch = "Roll";
intPitch = -1;
break;
}
switch (GyroForward)
{
case Base6Directions.Direction.Up:
strRoll = "Yaw";
intRoll = -1;
break;
case Base6Directions.Direction.Down:
strRoll = "Yaw";
intRoll = 1;
break;
case Base6Directions.Direction.Left:
strRoll = "Pitch";
intRoll = -1;
break;
case Base6Directions.Direction.Right:
strRoll = "Pitch";
intRoll = 1;
break;
case Base6Directions.Direction.Backward:
strRoll = "Roll";
intRoll = -1;
break;
case Base6Directions.Direction.Forward:
strRoll = "Roll";
intRoll = 1;
break;
}
}
