static void Simulate(PDController controller, ref Trajectory traj, Vector3d r, Vector3d v, Vector3d g, double dt, double maxT, string tgtLogFilename = "target.dat")
{
double t = 0;
double throttle;
Vector3d thrustV = Vector3d.zero;
Vector3d dr, dv, da; // desired values on trajectory
var tgtWriter = new System.IO.StreamWriter(tgtLogFilename);
tgtWriter.WriteLine("time x y z vx vy vz ax ay az att_err");
System.Console.WriteLine("time x y z vx vy vz ax ay az att_err");
while ((t < maxT) && (r.y > 0))
{
System.Console.WriteLine("{0:F2} {1:F2} {2:F2} {3:F2} {4:F2} {5:F2} {6:F2} {7:F2} {8:F2} {9:F2} 0", t, r.x, r.y, r.z, v.x, v.y, v.z, thrustV.x, thrustV.y, thrustV.z);
// Equations of motion
r = r + v * dt;
v = v + g * dt;
// Control loop
float att_err;
bool shutdownEnginesNow = false;
Vector3d att = new Vector3d(0, 1, 0); // TODO: Fake attitude
controller.GetControlOutputs(traj, r, v, att, g, (float)t, out dr, out dv, out da, out throttle, out thrustV, out att_err, out shutdownEnginesNow);
tgtWriter.WriteLine("{0:F2} {1:F2} {2:F2} {3:F2} {4:F2} {5:F2} {6:F2} {7:F2} {8:F2} {9:F2} {10:F2}", t, dr.x, dr.y, dr.z, dv.x, dv.y, dv.z, da.x, da.y, da.z, att_err);
// Make engine thrust have effect
v = v + thrustV * dt;
t = t + dt;
}
tgtWriter.Close();
}
public override void OnAttachToKoboto(Koboto koboto)
{
base.OnAttachToKoboto(koboto);
motorControl = new PDController(motorP, motorD);
bladeTransform.rotation = Quaternion.identity;
resetMotor = true;
dustVFX.SetActive(false);
}
// Update is called once per frame
void Update()
{
//First, analyze other robots to see which ones are too close
GameObject[] objects = GameObject.FindGameObjectsWithTag ("robot");
int ownID = this.gameObject.GetInstanceID ();
float minDistance = 10000;
float[] distances = new float[objects.Length];
//First compute minimum distance
for(int i=0;i<objects.Length;i++){
GameObject g = objects[i];
if (g.GetInstanceID () != ownID) { //Track others, not itself
float d = Vector3.Distance(transform.position,g.transform.position);
distances[i] = d;
if(d<minDistance)
minDistance = d;
}
}
//Update considering only closest neigbours
int j = 0;
lines.SetVertexCount (0); //Remove old lines
lines.SetVertexCount (6);
for (int i=0;i<objects.Length;i++){
GameObject g = objects[i];
int gID = g.gameObject.GetInstanceID();
AbstractController controller;
//See if we already have this controller in the dictionary
bool haveController = controllers.TryGetValue(gID,out controller);
if(!haveController){//If not, we include it
controller = new PDController(control);
controllers.Add(gID,controller);
}
if (g.GetInstanceID () != ownID) { //Track others, not itself
if(distances[i]<minDistance*(1+distanceEpsilon)){
Debug.DrawLine(transform.position,g.transform.position);
if(drawLines){
lines.SetPosition(j,transform.position);
j++;
lines.SetPosition(j,g.transform.position);
j++;
}
controller.controlFunction (transform, g.transform.position, targetDistance, Mathf.Deg2Rad*(-1), GlobalControl.motionModels.DYNAMIC);
}else{
//Reset controller
controller.setControl(control);
}
}
}
}
void Start()
{
myRigidbody = GetComponent <Rigidbody>();
controller = GetComponent <PDController>();
var speed = new LinguisticVariable("Speed");
//var slow = speed.MembershipFunctions.AddTrapezoid("Slow", 0, 0, 5, 10);
//var average = speed.MembershipFunctions.AddTrapezoid("Average", 10, 15, 20, 25);
//var fast = speed.MembershipFunctions.AddTrapezoid("Fast", 5, 15, 100, 100);
var slow = speed.MembershipFunctions.AddTriangle("Slow", 0, 5, 10);
var fast = speed.MembershipFunctions.AddTriangle("Fast", 7.5, 20, 50);
var distance = new LinguisticVariable("Distance");
/*
* var low = distance.MembershipFunctions.AddTrapezoid("Low", 0, 0, 2.5, 5);
* var medium = distance.MembershipFunctions.AddTrapezoid("Medium", 2.5, 5, 12.5, 20);
* var high = distance.MembershipFunctions.AddTrapezoid("High", 12.5, 20, 38, 50);
*/
var low = distance.MembershipFunctions.AddTriangle("Low", 0, 10, 20);
var high = distance.MembershipFunctions.AddTriangle("High", 10, 20, 55);
var variability = new LinguisticVariable("Variability");
var varH = variability.MembershipFunctions.AddTrapezoid("VarH", 1.5, 2, 5, 20);
var varL = variability.MembershipFunctions.AddTrapezoid("VarL", 0, 0, 1.5, 2);
fuzzyEngine = new FuzzyEngineFactory().Default();
var rule1 = Rule.If(speed.Is(slow)).Then(distance.Is(low));
//var rule2 = Rule.If(speed.Is(average)).Then(distance.Is(medium));
var rule3 = Rule.If(speed.Is(fast)).Then(distance.Is(high));
var rule4 = Rule.If(variability.Is(varL)).Then(distance.Is(low));
var rule5 = Rule.If(variability.Is(varH)).Then(distance.Is(high));
fuzzyEngine.Rules.Add(rule1);
//fuzzyEngine.Rules.Add(rule2);
fuzzyEngine.Rules.Add(rule3);
fuzzyEngine.Rules.Add(rule4);
fuzzyEngine.Rules.Add(rule5);
StartCoroutine(CycleAI());
}
public override void OnAttachToKoboto(Koboto koboto)
{
base.OnAttachToKoboto(koboto);
dragControl = new PDController(dragP, dragD);
chute.SetActive(false);
}
// Start is called before the first frame update
void Start()
{
myRigidbody = GetComponent <Rigidbody>();
controller = GetComponent <PDController>();
}
static int Main(string[] mainargs)
{
if (mainargs.Length == 0)
{
System.Console.Error.WriteLine("usage: VesselSim.exe - computes a solution given initial conditions like SolveText.exe but also simulates a vessel like running inside KSP");
System.Console.Error.WriteLine("vessel options: r0offset=<float>");
System.Console.Error.WriteLine(" v0offset=<float>");
System.Console.Error.WriteLine(" maxT=<float>");
return(1);
}
var solver = new Solve();
var result = new SolveResult();
var traj = new Trajectory();
var controller = new PDController();
bool correct = false;
Vector3d r0 = Vector3d.zero;
Vector3d v0 = Vector3d.zero;
Vector3d rf = Vector3d.zero;
Vector3d vf = Vector3d.zero;
Vector3d r0offset = Vector3d.zero; // move away from compute solution at start
Vector3d v0offset = Vector3d.zero;
int rfaxes = 0;
int vfaxes = 0;
double dt = 0.1;
double maxT = 0;
List <SolveTarget> targets = new List <SolveTarget>();
foreach (var arg in HGUtils.ToKeyValuePairs(mainargs))
{
bool used = solver.Set(arg.Item1, arg.Item2);
if (controller.Set(arg.Item1, arg.Item2))
{
used = true;
}
if (SetTargets(arg.Item1, arg.Item2, ref r0, ref v0, ref rf, ref vf, ref rfaxes, ref vfaxes, ref targets, ref correct))
{
used = true;
}
if (Set(arg.Item1, arg.Item2, ref dt, ref maxT, ref r0offset, ref v0offset))
{
used = true;
}
if (!used)
{
throw new System.ArgumentException("Unexpected argument: " + arg.Item1 + "=" + arg.Item2);
return(1);
}
}
SetFinalTarget(rf, vf, rfaxes, vfaxes, ref targets);
if (targets.Count == 0)
{
System.Console.Error.WriteLine("Error! You must specify targets with either target=[x,y,z], rf=[x,y,z] or vf=[x,y,z]");
return(1);
}
result = MainProg.MultiPartSolve(ref solver, ref traj, r0, v0, ref targets, (float)solver.g, solver.extendTime, correct);
if (result.isSolved())
{
List <string> comments = new List <string>();
System.Console.Error.WriteLine("Writing solution to: solution.dat");
traj.Write("solution.dat", comments);
if (traj.T > maxT)
{
maxT = traj.T;
}
r0 = r0 + r0offset;
v0 = v0 + v0offset;
Simulate(controller, ref traj, r0, v0, new Vector3d(0, -solver.g, 0), dt, maxT);
}
return(0);
}
private void Start()
{
controller = GetComponent <PDController>();
}
